🏎️ Mandol_ws: ROS 2 Autonomous Driving Workspace for the 2025 HL FMA aMAP Innovator Championship

This workspace contains the complete software stack for an autonomous vehicle, built on ROS 2 Humble. It was developed for the 2025 HL FMA aMAP Innovator Championship and features perception, localization, planning, and control modules, specifically designed for GPS-based navigation and mission execution.

Curve Navigation	Traffic Light Recognition	Perpendicular Parking	Parallel Parking

Below are the recordings of the autonomous driving runs from the final round.

Click on the thumbnails to watch the full driving videos on YouTube.

Run 1 (Rainy Conditions)	Run 2 (Sunny Conditions

---

🏆 Achievement

We secured 4th place among 44 participating teams and were honored with the Korea Road Traffic Authority Chairman's Award.

• Participating Teams List

• Award Results

  

---

🏁 2025 HL FMA aMAP Innovator Championship

1. 🚗 Competition Overview

The 2025 HL FMA (Future Mobility Award) aMAP Innovator Championship, supported by HL Group, is an autonomous driving competition that challenges participants to navigate a specialized driving course. The objective is to complete the course autonomously within a set time limit, while strictly adhering to traffic rules and executing specific maneuvers.

2. 📍 Event Details

• Location: Yongin Driver's License Examination Center

• Functional Check: September 27, 2025

  • Sensor and vehicle status verification.
  • Practice runs on specific courses (Refraction and Curve courses).

• Main Event: September 28, 2025

  • Full autonomous run of the license course.
  

3. 📋 Key Rules

• Time Limit: Total designated time is 8 minutes (reduced by 2 minutes from the previous year).

• No Human Intervention: After crossing the start line, any physical touch or remote input (Remote Control, Wireless Keyboard) results in Disqualification.

• Operational Constraint: Only one Operator (OP) can participate during the actual run.

• Stall Rule: Failure to move for more than 1 minute at any functional course leads to elimination.

• Scoring: The competition starts with a total of 150 points, and the team with the fewest deductions wins.

• Teams must prepare solutions for rainy conditions (waterproofing and sensor reliability).

  

4. ⚖️ Evaluation Criteria and Deductions

No.	Item	Criteria & Details	Deduction Points
1	Start	Failure to pass the start line within 1 minute after the signal.	5 pts
2	Lane Keeping	Any wheel touching or crossing centerlines, lane markers, or shoulder lines. (Evaluated from start to finish).	1 pt per instance (Max 15)
3	Emergency Stop	Failure to stop when an obstacle (model car) enters the lane. Distance between obstacle and car must be 2m or less (deduction if gap > 2m).	10 pts
4	Sloped Path	Must stop for 3 seconds or more in the detection zone. Rollback must be less than 50cm. Must pass within 30 seconds.	10 pts per violation
5	Intersections	Passing a red light (front bumper crossing the line). Stopping for more than 3 seconds past the line. Stalling inside intersection for 20+ seconds.	5 to 10 pts
6	Right-angle Parking	Entering forward, touching the line with rear wheels in the garage, and exiting. Failure to touch line or crossing boundaries.	10 pts per violation
7	Parallel Parking	Entering backward, touching the line with rear wheels, and exiting forward. Failure to touch line or crossing boundaries.	10 pts per violation
8	Time Limit	Every 1 minute exceeding the total 8-minute limit.	5 pts per min
9	Completion	The test ends when the rear wheels pass the finish line.	-

---

👥 Team Members

🧩 Members Name Major SeungMin Lee Mechanical Engineering DaeSeung Choi Applied Statistics SeungMin Yoo Smart Vehicle Engineering JunSeong Park Mechanical and Aerospace Engineering SeungHoon Ryu Mechanical, Robotic and Automotive Engineering HyeonSu Lee Mechanical, Robotic and Automotive Engineering 👑 Leadership Role Name Team Leader SeungMin Lee Technical Director DaeSeung Choi Mission Director SeungMin Yoo Hardware Director JunSeong Park

---

💻 Role Assignments

Domain	Members
📷 Camera	SeungMin Lee
🌐 GPS	DaeSeung Choi, SeungMin Yoo, HyeonSu Lee
🗺️ Path Planning	DaeSeung Choi, HyeonSu Lee
🧠 Decision	SeungMin Yoo, DaeSeung Choi
🕹️ Control	JunSeong Park, SeungHoon Ryu
⚙️ Hardware	JunSeong Park, SeungHoon Ryu

---

⭐ Core Architecture

Component	Description
📡 Sensing	A dual u-blox GPS setup provides RTK-corrected localization. The antenna for the F9P is on the front axle, while the antenna for the more performant F9R is on the rear axle to enhance the accuracy of the rear-axle-based pure pursuit control. Both antennas are the u-blox ANN-MB-01 model. A forward-facing USB camera is used for traffic light detection, while an Intel RealSense depth camera is used for obstacle detection.
📍 Localization	The gps_to_utm package converts raw GPS data to local UTM coordinates. The tf_gps_csv_node reads a pre-recorded path and continuously broadcasts the vehicle's position and orientation (/tf) relative to that path's coordinate frame, effectively placing the vehicle on the map. Vehicle heading is precisely calculated by the azimuth_angle_calculator_node using the spatial vector between the two GPS receivers.
🗺️ Planning	The path_planning package is responsible for path following. The f9r_roi_path node extracts a local path (Region of Interest) from the global map. The pure_pursuit_node then calculates the required steering angle to follow this local path, featuring a dynamic lookahead distance that adapts to path curvature for optimal performance. The package also includes the csv_radius_detector_node, which publishes boolean triggers when the vehicle enters predefined mission zones (e.g., for parking).
👁️ Perception	The mando_vision node processes sensor data in parallel. It uses a YOLO model on the USB camera stream to detect traffic lights (publishing /traffic_stop) and another YOLO model on the RealSense camera stream to detect obstacles. For obstacles, it uses the aligned depth map to determine their distance and publishes a boolean /obstacle_existance signal.
🧠 Decision	The mission_supervisor node acts as the vehicle's brain, implementing a robust, two-layer state machine. A high-priority Safety Layer can preemptively halt the vehicle in response to signals like /obstacle_existance or /slope_stop. A lower-priority Mission Layer manages the sequence of driving tasks (e.g., GPS_FWD, REVERSE_T) based on triggers from the planning system. This node arbitrates all commands to produce the final, safe vehicle instructions.
🕹️ Control	Control is split into two levels: - High-Level Control (serial_bridge): This ROS 2 node acts as the final link in the software stack, translating the arbitrated /throttle_cmd and /auto_steer_angle topics from the mission_supervisor into simple serial commands (e.g., "TH 0.5", "SA -15.2"). - Low-Level Control (Arduino): An Arduino microcontroller running the MANDO_FINAL_NO_MANUAL.ino firmware receives commands from the serial_bridge. It executes them using a closed-loop P-controller for the steering (using potentiometer feedback for high precision) and an open-loop controller for the throttle. It also includes a critical failsafe that stops the vehicle if serial commands are not received within a 1-second timeout.

---

🚀 Initial Setup

ROS 2 Dependencies

sudo apt update
sudo apt install ros-humble-rtcm-msgs ros-humble-nmea-msgs
sudo apt install ros-humble-tf-transformations
sudo apt install ros-humble-ackermann-msgs ros-humble-nav2-bringup

GPS Open Source Packages

This project utilizes modified versions of the following open-source packages for GPS handling.

• u-blox Driver: https://github.com/olvdhrm/RTK_GPS_NTRIP
• NTRIP Client: https://github.com/SGroe/ntrip_client_ros2

---

🗺️ Path Visualization Utility

Create CSV from rosbag

• Node: f9r_to_csv.py from gps_to_utm package.
• Command: ros2 run gps_to_utm f9r_to_csv
• Description: A utility script to extract /f9r/fix messages from a rosbag file and convert the latitude/longitude data into a UTM-based CSV file for path planning.

Visualize CSV Paths

• Script: visualize_all_csv.py

• Description: A helper script to plot and visualize all generated CSV path files using matplotlib.

  

Check GPS Topic Frequency

It's crucial that the GPS topics are publishing at a sufficient rate. Use the following commands to verify.

ros2 topic hz /f9p/fix
ros2 topic hz /f9r/fix

---

⌨️ Execution Procedure

1. 🔌 Sensor Connection and Initial Settings

1. Connect only the Arduino to a USB port.

2. Verify it is assigned to /dev/ttyACM0 using ls /dev/ttyACM* /dev/ttyUSB*.

3. Upload the Arduino sketch. If it fails, a running serial_bridge node might be holding the port. Stop the node, upload, and then restart the node.

4. Run the Serial Bridge Node.

   ros2 run serial_bridge serial_bridge

   Node Description: serial_bridge

   • Purpose: Acts as a bridge between high-level ROS 2 command topics and a low-level hardware controller (Arduino) connected via a serial port. It translates ROS messages into simple text commands that the microcontroller can understand.
   • Subscriptions:
     • /throttle_cmd (std_msgs/msg/Float32): The final, arbitrated throttle command.
     • /auto_steer_angle (std_msgs/msg/Float32): The final steering command from the path planner.
   • Publications: None. Its output is to the serial port.
   • Core Logic:
     1. Connection: Continuously attempts to connect to the Arduino on the specified serial port and baud rate. Automatically retries on failure.
     2. Command Forwarding: When a message is received on /throttle_cmd or /auto_steer_angle, it formats it into a string (e.g., "TH 0.5\n", "SA -15.2\n") and sends it over the serial port.
     3. Reception: Reads any lines sent from the Arduino and prints them to the ROS log, which is useful for debugging.
     4. Safety: A startup_silence_sec parameter prevents commands from being sent for a few seconds after startup, avoiding unintended vehicle movement.

5. Connect the u-blox F9P GPS receiver.

   

6. Verify it is assigned to /dev/ttyACM1 using ls /dev/ttyACM* /dev/ttyUSB*.

7. Launch the F9P Driver Node.

   ros2 launch ublox_gps ublox_f9p_launch.py

8. Connect the u-blox F9R GPS receiver.

   

9. Verify it is assigned to /dev/ttyACM2 using ls /dev/ttyACM* /dev/ttyUSB*.

10. Launch the F9R Driver Node.

    ros2 launch ublox_gps ublox_f9r_launch.py

    Node Description: ublox_gps_node (launched by ublox_f9p_launch.py and ublox_f9r_launch.py)

    • Purpose: A highly configurable driver for u-blox GPS receivers. It communicates with the hardware, decodes various UBX messages, and publishes standard ROS messages.
    • Subscriptions:
      • ~/rtcm (rtcm_msgs/Message): Subscribes to RTCM correction data from an NTRIP client (like /ntrip_client/rtcm). This is essential for achieving RTK-level accuracy.
    • Publications:
      • ~/fix (sensor_msgs/msg/NavSatFix): Publishes the primary GPS fix containing latitude, longitude, altitude, and status. This is remapped to /f9p/fix or /f9r/fix.
      • Other topics for velocity (/navvel), satellite info, etc., are also available.
    • Core Logic:
      1. Initializes communication with the u-blox device over a serial port specified in the launch file.
      2. Configures the device based on a YAML file (e.g., setting the publishing rate of different UBX messages).
      3. Receives RTCM data from the ~/rtcm topic and forwards it to the GPS device to enable RTK corrections.
      4. Parses the resulting navigation data from the device and publishes it on various ROS topics.

2. 🛰️ RTK GPS Configuration

11. Run the Fix-to-NMEA Converter Node.

    ros2 run fix2nmea fix2nmea

    Node Description: fix2nmea

    • Purpose: A small utility node that serves as a bridge between the ublox_gps_node and the ntrip_client. The NTRIP protocol requires the vehicle's current position in the NMEA (GNGGA) format to request correction data, but the ublox_gps_node publishes a NavSatFix message. Thus, this node is required to perform that conversion.
    • Subscriptions:
      • ~/fix (sensor_msgs/msg/NavSatFix): Subscribes to the GPS fix from the u-blox driver. The topic is remapped in its launch file to /f9p/fix.
    • Publications:
      • ~/nmea (nmea_msgs/msg/Sentence): Publishes the vehicle's position as a standard GNGGA NMEA sentence. This is subscribed to by the ntrip_client.
    • Core Logic:
      1. Receives a NavSatFix message.
      2. Extracts latitude, longitude, and other details.
      3. Formats these details into a valid GNGGA NMEA string.
      4. Publishes the string in a Sentence message.

12. Configure NTRIP Mountpoint: Check and edit the launch file /home/hannibal/Mandol_ws/src/RTK_GPS_NTRIP/ntrip_client/launch/ntrip_client_launch.py and change DeclareLaunchArgument('mountpoint', ...) to match the RTK base station for your current location (e.g., from SUWN-RTCM31 to SONP-RTCM31).

13. Connect to a mobile hotspot for internet access.

14. Launch the NTRIP Client for RTK corrections. The node will automatically try to reconnect if errors occur.

    ros2 launch ntrip_client ntrip_client_launch.py

    Node Description: ntrip_client

    • Purpose: Connects to an NTRIP caster (an internet server providing RTK correction data). It sends the vehicle's position and receives a stream of correction data, which it then publishes for the GPS driver.
    • Subscriptions:
      • ~/nmea (nmea_msgs/msg/Sentence): Subscribes to the NMEA sentence from fix2nmea containing the vehicle's current position.
    • Publications:
      • ~/rtcm (rtcm_msgs/msg/Message): Publishes the raw RTCM correction data received from the NTRIP caster. This is subscribed to by the ublox_gps_node.
    • Core Logic:
      1. Connects to the NTRIP caster specified by host, port, mountpoint, and credentials in the launch file.
      2. Forwards incoming NMEA messages to the caster to request data.
      3. Continuously polls the NTRIP socket for incoming RTCM data.
      4. Publishes received RTCM data packets as ROS messages.
      5. Includes robust logic for automatic reconnection if the internet connection drops.
    • RTK Verification:
      • If the status for both F9P and F9R appears as 2 in PlotJuggler, it indicates that RTK is active on both units.

---

3. 🌐 GPS Coordinate Transformation and TF Broadcasting

The following nodes work together to transform raw GPS data into a local metric coordinate system (utm) and establish the relationship between the pre-recorded map and the vehicle's real-time position using TF (transform frames).

Note: The images displayed below were captured during experiments on a custom practice track set up at Konkuk University's Open-Air Theater, not at the actual competition venue.

15. Configure CSV Map for TF: Specify the global path map to be used in /home/hannibal/Mandol_ws/src/gps_to_utm/config/tf_gps_csv.yaml. This map is used to establish the local coordinate system.

    

16. Configure Mission Areas: Specify the location of mission trigger zones in /home/hannibal/Mandol_ws/src/path_planning/config/csv_detector.yaml. This file defines the center and radius of areas for T-parking, parallel parking, etc., corresponding to the chosen global path.

    

17. Run the F9P to UTM Converter.

    ros2 run gps_to_utm f9p_to_utm

    Node Description: f9p_to_utm

    • Purpose: Converts raw GPS coordinates (latitude, longitude) from the F9P sensor into UTM (Universal Transverse Mercator) coordinates, which are metric and suitable for local path planning.
    • Subscriptions:
      • /f9p/fix (sensor_msgs/msg/NavSatFix): The GPS fix data from the u-blox F9P receiver.
    • Publications:
      • /f9p_utm (geometry_msgs/msg/PointStamped): The converted UTM coordinates (easting, northing, altitude) in a metric frame named utm.

18. Run the F9R to UTM Converter.

    ros2 run gps_to_utm f9r_to_utm

    Node Description: f9r_to_utm

    • Purpose: Identical to f9p_to_utm, but for the F9R sensor.
    • Subscriptions:
      • /f9r/fix (sensor_msgs/msg/NavSatFix): The GPS fix data from the u-blox F9R receiver.
    • Publications:
      • /f9r_utm (geometry_msgs/msg/PointStamped): The converted UTM coordinates from the F9R sensor.

19. Run the Azimuth Angle Calculator.

    ros2 run gps_to_utm azimuth_angle_calculator_node

    Node Description: azimuth_angle_calculator_node

    • Purpose: Calculates the vehicle's heading (azimuth angle) in degrees using the positions of the two GPS receivers (front and rear). It adjusts the heading by 180 degrees during reverse missions to support the Pure Pursuit algorithm.

    • Azimuth angle: Represents the vehicle's current heading relative to True North, measured in degrees.

    • Subscriptions:

      • /f9r/fix (sensor_msgs/msg/NavSatFix): Position of the rear GPS (Point 1).
      • /f9p/fix (sensor_msgs/msg/NavSatFix): Position of the front GPS (Point 2).
      • /mission_state (std_msgs/msg/String): The current mission state, used to detect reverse driving.

    • Publications:

      • /azimuth_angle (std_msgs/msg/Float64): The calculated vehicle heading in degrees (0-360).

    • Core Logic:

      1. Synchronization: The node waits for fresh messages from both GPS topics and ensures their timestamps are within a close range (e.g., 0.1s) to prevent calculations based on unsynchronized data.

      2. Bearing Calculation: It calculates the forward bearing from the rear GPS to the front GPS using their latitude (lat) and longitude (lon) coordinates. This is done using the spherical law of cosines for calculating bearing:

         • Let Point 1 (rear GPS) be (lat1, lon1) and Point 2 (front GPS) be (lat2, lon2).
         • First, convert all coordinates from degrees to radians.
         • Then, calculate the change in longitude: Δlon = lon2 - lon1.
         • The core formula is:

           Y = sin(Δlon) * cos(lat2)
           X = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(Δlon)
           Bearing (radians) = atan2(Y, X)
           

      3. Normalization: The resulting angle in radians is converted to degrees and normalized to a 0-360 degree range.

      4. Reverse Logic: The node subscribes to /mission_state. If the state is REVERSE_T or REVERSE_PARALLEL, it adds 180 degrees to the final calculated heading.

      5. Why 180?: The Pure Pursuit algorithm is designed for forward driving. By flipping the heading 180 degrees during reverse missions, the controller perceives the vehicle as "facing" the path segment that is actually behind it. This trick allows the same forward-driving steering logic to be used to successfully guide the vehicle while backing up.

         Before adding 180°	After adding 180°
         Even though reversing is required, the yellow arrow (facing) still points forward.	In the reverse situation, the yellow arrow (facing) is flipped to point backward.

      6. Publication: The final heading (either normal or flipped) is published to the /azimuth_angle topic.

20. Run the CSV Path and TF Broadcaster. You can either run the node directly after configuring the YAML or use the launch file.

    # Option A: Run node directly (requires rebuild on config change)
    ros2 run gps_to_utm tf_gps_csv_node
    
    # Option B: Use launch file (no rebuild needed for config change)
    ros2 launch gps_to_utm tf_gps_csv.launch.py

    Node Description: tf_gps_csv_node

    • Purpose: A key localization node that reads a pre-recorded path from a CSV file, continuously publishes it for planners, and broadcasts the vehicle's position as a TF transform relative to that path.
    • Configuration: The path to the global map is specified using the csv_file_path parameter in /home/hannibal/Mandol_ws/src/gps_to_utm/config/tf_gps_csv.yaml.
    • Subscriptions:
      • /f9r_utm (geometry_msgs/msg/PointStamped): Real-time UTM position of the rear GPS.
      • /f9p_utm (geometry_msgs/msg/PointStamped): Real-time UTM position of the front GPS.
      • /azimuth_angle (std_msgs/msg/Float64): Vehicle's heading, used for the orientation of the TF frame.
    • Publications:
      • /csv_path (nav_msgs/msg/Path): The global path loaded from the CSV file. Published with a transient local QoS to ensure late-joining nodes receive it.
      • /tf (tf2_msgs/msg/TFMessage): Broadcasts the transform for the f9r and f9p frames relative to a csv parent frame, effectively placing the vehicle on the map.
      • /azimuth_angle_text (visualization_msgs/msg/Marker): A text marker for Rviz showing the current heading.
    • Core Logic:
      1. Loads the CSV file specified by the csv_file_path parameter. It uses the first point in the file as the origin (0,0) of the csv coordinate frame.
      2. Publishes all points from the CSV as a Path message on a 20 Hz timer.
      3. When it receives UTM points for the GPS sensors, it calculates their position relative to the CSV origin and broadcasts their transforms, using the latest /azimuth_angle for orientation.

• All-in-One Launch File: The four nodes above (17-20) can be started together with a single launch file.

  ros2 launch gps_to_utm total.launch.py

  

---

4. 🗺️ Path Planning

These nodes are responsible for generating a local path to follow and calculating the commands to do so. Note: Restart these nodes every time you begin a new run.

21. Run the ROI Path Publisher.

    # Option A: Run node directly
    ros2 run path_planning f9r_roi_path
    
    # Option B: Use launch file
    ros2 launch path_planning f9r_roi_path.launch.py

    Node Description: f9r_roi_path

    • Purpose: Generates local "Region of Interest" (ROI) paths for both forward and reverse driving by extracting segments from the global CSV path relative to the vehicle's position.

    • Subscriptions:

      • /csv_path (nav_msgs/msg/Path): The global path from tf_gps_csv_node.
      • /f9r/fix (sensor_msgs/msg/NavSatFix): Used to locate the vehicle on the path.

    • Publications:

      • /f9r_front_roi_path (visualization_msgs/msg/Marker): Red LINE_STRIP marker representing the path in front of the vehicle (for GPS_FWD).
      • /f9r_front_roi_end (visualization_msgs/msg/Marker): Red sphere indicating the end of the front ROI.

    • Core Logic: The node's core logic is designed to be highly robust, preventing the vehicle from losing its place on the global path, especially at intersections or where paths run close together. This is achieved through a two-phase process and strict index discipline.

      1. Phase 1: Initial Path Lock-On

         • On its very first run, the node performs a wide search forward along the global path (using the search_span_points parameter, e.g., 2000 points).
         • It finds the closest point to the vehicle within this large area. This initial, broad search ensures the vehicle correctly "locks on" to the intended path at the beginning of a run.

      2. Phase 2: Constrained ROI Generation (The "No-Jump" Constraint)

         • After the first ROI has been published, the node switches to a much stricter mode for all subsequent updates.
         • To find the new starting point for the next ROI, it only searches within the points of the previously generated ROI.
         • This is the critical "jump prevention" feature: by constraining the search space, the node cannot mistakenly find a "closer" point on an adjacent lane or a different section of the track.

      3. Strictly Forward Progression

         • The node guarantees that the starting index of the ROI on the global path can never decrease. It always takes the maximum of the newly found closest index and the previous starting index.
         • This ensures the vehicle only ever moves forward along the path, preventing it from getting stuck or trying to go backward.

      4. ROI Extraction:

         • From the validated starting point, it iterates forward along the global path for a specified distance (e.g., 7 meters) or a set number of points to generate the final local path (/f9r_front_roi_path) for the controller.
         

22. Run the Pure Pursuit Controller.

    # Option A: Run node directly
    ros2 run path_planning pure_pursuit_node
    
    # Option B: Use launch file
    ros2 launch path_planning pure_pursuit.launch.py

    Node Description: pure_pursuit_node

    • Purpose: Calculates steering angles and throttle commands using the Pure Pursuit algorithm. It adapts to both forward and reverse driving by selecting the appropriate ROI path and heading.
    • Subscriptions:
      • /f9r_front_roi_path (visualization_msgs/msg/Marker): The local paths to follow.
      • /mission_state (std_msgs/msg/String): Determines whether to use the front or back ROI and whether to use fixed or dynamic lookahead.
      • /azimuth_angle (std_msgs/msg/Float64): The vehicle's heading (already flipped by 180° if reversing).
    • Publications:
      • /auto_steer_angle (std_msgs/msg/Float32): The calculated steering command (in degrees).
      • /throttle_from_planning (std_msgs/msg/Float32): A suggested throttle value, which is lower on sharp turns.
      • /pp/lookahead_point (visualization_msgs/msg/Marker): A sphere visualizing the target point the vehicle is steering towards.
      • /pp/steering_arrow (visualization_msgs/msg/Marker): An arrow indicating the desired heading vector.
      • /pp/curvature_circle (visualization_msgs/msg/Marker): A circle representing the curvature of the current turn.
    • Core Logic:

      1. Mode Selection: Checks /mission_state.

         • Forward: Uses front_roi_path.
         • Reverse: Uses back_roi_path and the 180°-flipped azimuth_angle.

      2. Dynamic Lookahead (LD) Logic: The node implements a curvature-adaptive lookahead distance to optimize path tracking.

         • Input Data: It subscribes to the ROI path topic (e.g., /f9r_front_roi_path), which is a visualization_msgs/msg/Marker. It analyzes the geometry of the points within this path to determine the upcoming curvature.
         • Curvature Calculation (localCurvatureAbsAvg function):
           1. Sliding Window: It considers a window of points on the path around the vehicle's current closest point.
           2. Menger Curvature: For each set of three consecutive points (p1, p2, p3) in the window, it forms a triangle and calculates its curvature (κ) using the Menger curvature formula: κ = 4 * area / (dist(p1,p2) * dist(p2,p3) * dist(p3,p1)). This formula finds the curvature of the circle that passes through the three points.
           3. Averaging: The absolute curvature values calculated within the window are averaged to get a representative local path curvature.
           4. Smoothing (EMA Filter): The averaged curvature value is then smoothed using an Exponential Moving Average (EMA) filter. This prevents abrupt changes and ensures the final curvature value (kappa_filt_) is stable.
         • Adaptive LD Algorithm:
           • The final, smoothed curvature is used in an exponential decay function: Ld = min_ld + (max_ld - min_ld) * exp(-beta * kappa_filt_).
           • High Curvature (Sharp Turns): When kappa_filt_ is large, the exp() term approaches zero, so Ld decreases towards min_ld. This allows for precise tracking around corners.
           • Low Curvature (Straight Lines): When kappa_filt_ is near zero, the exp() term approaches one, so Ld increases towards max_ld. This provides smooth, stable control on straightaways.
         • Benefit: This dynamic adjustment provides high tracking accuracy in tight maneuvers and smooth, stable control at higher speeds on straightaways.
         • Exception for Reverse: For reverse missions (REVERSE_T, REVERSE_PARALLEL), this dynamic logic is bypassed. A short, fixed lookahead distance (back_ld_) is used to ensure stability and high precision during sensitive backup maneuvers.

      3. Steering Calculation: It computes the steering angle required to intercept the target point. By using the back_roi_path and the pre-flipped azimuth angle during reverse, the standard Pure Pursuit equations correctly guide the vehicle while backing up.

      4. Throttle Calculation: A suggested throttle value is calculated, which is inversely proportional to the required steering angle (i.e., slows down on sharp turns).

      5. Publishes the final steering and throttle commands.

         

23. Run the Mission Area Detector.

    # Option A: Run node directly
    ros2 run path_planning csv_radius_detector_node
    
    # Option B: Use launch file
    ros2 launch path_planning csv_detector.launch.py

    Node Description: csv_radius_detector_node

    • Purpose: Defines geographic "mission areas" (circles) and publishes a boolean topic for each, indicating if the vehicle is inside. This is used to trigger missions like parking.
    • Configuration: Mission areas are defined in /home/hannibal/Mandol_ws/src/path_planning/config/csv_detector.yaml. To add a new area, first add its name to the area_names list. Then, create a new section with that name defining its state_topic, viz_topic, center coordinates (x, y in UTM), and radius in meters.
    • Subscriptions:
      • /f9r_utm (geometry_msgs/msg/PointStamped): The vehicle's real-time UTM position.
    • Publications (dynamically created from parameters):
      • <area_name>/trigger (std_msgs/msg/Bool): Publishes true when the vehicle is inside the named area.
      • <area_name>/viz (visualization_msgs/msg/Marker): Publishes a marker to visualize the area in Rviz.
    • Core Logic:
      1. Loads the list of mission areas from the YAML file.
      2. For each area, it creates a boolean publisher and a marker publisher.
      3. On receiving a /f9r_utm message, it calculates the vehicle's distance to the center of each area.
      4. If the distance is less than an area's radius, it publishes true on that area's trigger topic; otherwise, it publishes false.

---

5. 👁️ Vision System Execution

24. USB udev Trigger: Ensure the system recognizes the USB devices properly.

    sudo udevadm trigger 

25. Launch USB Camera: Load the settings and run the driver for the traffic light camera.

    ros2 run usb_cam usb_cam_node_exe --ros-args --remap __ns:=/camera2 --params-file /home/hannibal/Mandol_ws/src/usb_cam/config/params_2.yaml

    Node Description: usb_cam_node_exe

    • Purpose: A general-purpose ROS 2 node for streaming video from V4L2-compatible USB cameras.
    • Publications:
      • image_raw or image_raw/compressed (sensor_msgs/msg/Image or CompressedImage): The video stream.
      • camera_info (sensor_msgs/msg/CameraInfo): Camera calibration data.
    • Note: In this workspace, it is remapped to the /camera2 namespace.

26. Launch RealSense Camera: Start the driver for the Intel RealSense depth camera.

    ros2 launch realsense2_camera my_realsense.launch.py

27. Run the Vision Perception Node.

    ros2 run mando_vision vision_node

    Node Description: vision_node

    • Purpose: A unified perception node that runs two parallel AI-based detection tasks: traffic light detection and obstacle detection.
    • Traffic Light Module:
      • Input: /camera2/image_raw/compressed (USB Camera)
      • Output: /traffic_stop (std_msgs/msg/Bool)
      • Logic: Uses a YOLO model to detect red, green, yellow, and left signals. A robust state machine determines the final stop signal.
        • State Machine: To prevent flickering, a signal must be detected for a few consecutive frames to be confirmed, and it must be lost for several frames to be cleared.
        • Go Signals: green and yellow lights are treated as "go" signals, immediately clearing any stop state. yellow is explicitly not a stop condition.
        • Left Turn Grace Period: This is a key feature for robust navigation. When a left signal is detected, the node enters a "grace period" for a few seconds. During this period, any subsequent red light detection is deliberately ignored. This prevents the vehicle from stopping incorrectly after making a legal left turn when the signal immediately turns red.
    • Obstacle Detection Module:
      • Input: Synchronized /camera/color/image_raw/compressed and /camera/aligned_depth_to_color/image_raw (Intel RealSense).
      • Output: /obstacle_existance (std_msgs/msg/Bool)
      • Logic: Uses a YOLO model to detect obstacles. It then calculates their real-world distance with high accuracy.
        1. Detection: Identifies the highest-confidence obstacle from the color image.
        2. Robust Distance Measurement: Instead of relying on a single pixel, it takes a small Region of Interest (ROI) from the center of the obstacle's bounding box on the raw depth map. The median depth value within this ROI is used, which effectively filters out noise and outlier depth readings.
        3. 3D Transformation: The obstacle's 2D position and median depth are first converted to a 3D point in the camera's coordinate system. This point is then transformed into the vehicle's base_link frame using the TF tree.
        4. Thresholding: If the final calculated distance to the obstacle in the base_link frame is less than a predefined threshold, it publishes true.
    • Visualization: Publishes annotated video streams for both cameras under /unified_vision/....

---

6. 🧠 Mission Supervisor

28. Run the Mission Supervisor Node.

    ros2 run mission_supervisor mission_supervisor_node

    Node Description: mission_supervisor

    This node implements a two-layer, preemptive state machine to ensure safe and reliable mission execution. It separates safety-critical stops from mission-specific logic.

    🛡️ Safety Layer (Preemptive Hold)

    This layer has the highest priority. If any stop signal becomes active, it immediately takes control, holds the vehicle, and only returns control to the Mission Layer after the stop signal is cleared and a hold/hysteresis condition is met.

    Signal/State	Trigger Condition	Priority	Hold Action	Release Condition
    SAFE_OK	All STOP_* signals are false	-	No preemption	-
    STOP_SLOPE	slope_stop == true	1	throttle = 0.15 (slope correction)	Hold for 5s + signal becomes false
    STOP_OBSTACLE	obstacle_existance == true	2	throttle = 0	Hold for 5s + signal becomes false
    STOP_TRAFFIC	traffic_stop == true AND intersection == true	3	throttle = 0	Hysteresis + signal becomes false

    flowchart LR
      SAFE_OK["SAFE_OK (no stop signals)"]
      STOP_SLOPE["STOP_SLOPE (slope_stop == true)"]
      STOP_OBSTACLE["STOP_OBSTACLE (obstacle_existance == true)"]
      STOP_TRAFFIC["STOP_TRAFFIC (traffic_stop == true && intersection == true)"]
      SAFETY_HOLD["SAFETY_HOLD (throttle = 0 or 0.15 on slope)"]
      PREV["PREV_MISSION (return after release)"]
    
      STOP_SLOPE -->|priority 1 - hold >= 5s| SAFETY_HOLD
      STOP_OBSTACLE -->|priority 2 - hold >= 5s| SAFETY_HOLD
      STOP_TRAFFIC -->|priority 3 - hysteresis| SAFETY_HOLD
    
      SAFETY_HOLD -->|signals false + hysteresis| PREV
      SAFE_OK -->|no preemption| PREV
    

    Loading

    ---

    🚦 Mission Layer

    This layer manages the sequence of driving tasks. It transitions between states based on triggers from the csv_radius_detector. The Safety Layer can preempt it at any time.

    State	Entry Trigger	Active Action	Exit Trigger
    INIT	Node start	Initialize	Auto-transition to GPS_FWD
    GPS_FWD	From INIT or mission completion	Follow path using Pure Pursuit	/reverse_T/trigger or /reverse_parallel/trigger
    REVERSE_T	/reverse_T/trigger == true	Execute T-parking reverse sequence	/reverse_T/done == true
    REVERSE_PARALLEL	/reverse_parallel/trigger == true	Execute parallel parking reverse sequence	/reverse_parallel/done == true
    SAFETY_HOLD (Preemptive)	Any STOP_* signal is true	Overrides throttle (e.g., to 0)	All STOP_* signals false + hysteresis

    stateDiagram-v2
      INIT --> GPS_FWD: start
    
      state "GPS_FWD (FWD_CONTROLLER)" as GPS_FWD
      state "REVERSE_T (REVERSE_T_CONTROLLER)" as REVERSE_T
      state "REVERSE_PARALLEL (REVERSE_PARALLEL_CONTROLLER)" as REVERSE_PARALLEL
      state "SAFETY_HOLD (preemptive hold)" as SAFETY_HOLD
    
      %% Main transitions
      GPS_FWD --> REVERSE_T: /reverse_T/trigger
      REVERSE_T --> GPS_FWD: /reverse_T/done
      GPS_FWD --> REVERSE_PARALLEL: /reverse_parallel/trigger
      REVERSE_PARALLEL --> GPS_FWD: /reverse_parallel/done
    
      %% Safety preemption (from any mission state)
      GPS_FWD --> SAFETY_HOLD: any STOP_* active
      REVERSE_T --> SAFETY_HOLD: any STOP_* active
      REVERSE_PARALLEL --> SAFETY_HOLD: any STOP_* active
    
      %% Recovery to previous mission after release
      SAFETY_HOLD --> GPS_FWD: release -> prev_mission
    

    Loading

    ---

    • Subscriptions:
      • /slope_stop, /obstacle_existance, /traffic_stop, /intersection (std_msgs/msg/Bool): Inputs to the Safety Layer.
      • /reverse_T/trigger, /reverse_parallel/trigger (std_msgs/msg/Bool): Entry triggers for mission states.
      • /reverse_T/done, /reverse_parallel/done (std_msgs/msg/Bool): Exit triggers for mission states.
      • /throttle_from_planning (std_msgs/msg/Float32): The suggested throttle from the pure pursuit controller.
    • Publications:

      • /throttle_cmd (std_msgs/msg/Float32): The final, arbitrated throttle command sent to the serial_bridge.

      • /mission_state (std_msgs/msg/String): The current state of the Mission Layer (e.g., "GPS_FWD").

        

---

🕹️ Arduino Controller Strategy

The project utilizes two different Arduino firmwares, each for a specific purpose. This separation was necessary due to competition regulations which prohibited the use of remote controllers.

• MANDO_FINAL_DYNAMIC_TH_SECOND.ino (Practice Firmware): Used for practice and development. For convenience, this version supports dual-mode control: manual driving via an RC remote and autonomous control via serial commands from the main computer.

• MANDO_FINAL_NO_MANUAL.ino (Competition Firmware): Used during the official competition. To comply with regulations, the vehicle's RC receiver was physically removed, and this firmware, which contains no RC-related code, was used. It is a hardened, autonomous-only firmware with an added serial communication failsafe.

---

1. 🎮 Practice & Development Firmware (MANDO_FINAL_DYNAMIC_TH_SECOND.ino)

This sketch provides the flexibility to switch between manual and autonomous control, making it ideal for testing and debugging in a non-competition environment.

• Primary Feature: Dual-Mode Control

  • The firmware actively reads signals from an RC receiver connected to the Arduino.

  • Based on the signal from a specific channel on the RC transmitter, it switches between three modes:

    1. MANUAL_MODE: The vehicle is controlled directly by the RC transmitter's joysticks. This is used for easy positioning and manual intervention.

    2. AUTO_MODE: The vehicle ignores the RC transmitter and only accepts TH (throttle) and SA (steering angle) commands sent over the serial port from the ROS serial_bridge node.

    3. BREAK_MODE: A safety state that stops all motors.

• Control Logic

  • Steering Control (P-Controller): The precise, closed-loop P-control for steering (using potentiometer feedback) is active in both Manual and Auto modes. The only difference is the source of the target angle (RC remote vs. serial command).

  • Throttle Control (Open-Loop): In both Manual and Auto modes, the throttle command (from the RC remote or serial) is directly mapped to a motor PWM value.

  • The wheel encoder is not used for speed feedback.

• Key Weakness: No Serial Failsafe

  • This version lacks the serial communication timeout feature. In AUTO_MODE, if the connection to the main computer is lost, the Arduino will continue executing the last received command indefinitely. This makes it less safe for fully autonomous operation compared to the competition firmware.

---

2. 🚫🎮 Competition Firmware (MANDO_FINAL_NO_MANUAL.ino)

This is the hardened, autonomous-only version of the firmware used during the official competition runs. It is the lowest-level component in the vehicle's control hierarchy when running autonomously.

Core Responsibilities

1. Command Reception: Parses newline-terminated commands (TH for throttle, SA for steering angle) from the main computer.

2. Actuator Control: Directly drives the motors for steering and propulsion.

3. Closed-Loop Steering: Implements a PID (Proportional-Integral-Derivative) controller to ensure the steering angle precisely matches the command.

4. Open-Loop Throttle: Sets the drive motor power based on the throttle command without speed feedback.

5. Failsafe: Automatically stops the vehicle if commands are not received within a specified timeout.

Hardware Interface

• Serial Port: Communicates with the serial_bridge at 115200 baud.

• Drive Motors: Controlled via DIR1/PWM1 (pins 10/11) and DIR2/PWM2 (pins 6/7).

• Steering Motor: A DC motor controlled by DIR3/PWM3 (pins 8/9).

• Steering Feedback: A potentiometer on analog pin A0 provides crucial feedback on the actual, current steering angle.

• Wheel Encoder: A quadrature encoder is connected to pins 18 and 19 and its interrupt service routine (ISR) counts wheel rotations. Note: While the encoder counts are logged, they are not used for speed control in this version of the code; the throttle control is purely open-loop.

Serial Communication Protocol

The sketch expects simple, newline-terminated ASCII commands:

• TH <value>: Sets the throttle. <value> is a float between -1.0 (full reverse) and 1.0 (full forward).

• SA <value>: Sets the desired steering angle. <value> is a float in degrees, clamped to the vehicle's maximum range (e.g., -20° to +20°).

A critical safety feature is the command timeout. If a valid TH or SA command is not received within 1 second (serialMS_TIMEOUT), the controller assumes a communication failure and immediately calls StopMotor() to halt the vehicle.

Control Logic Explained

Steering Control (Closed-Loop, P-Controller)

The steering system is a true closed-loop feedback system, ensuring high precision.

1. Goal: Make the actual angle of the wheels match the desired angle from the SA command.

2. Setpoint: The desired angle received from the SA command (e.g., 15.0 degrees).

3. Process Variable: The current, real-world steering angle is continuously measured by reading the potentiometer on pin A0. The raw analog value is mapped to a calibrated degree range (e.g., 34-978 -> +20° to -20°).

4. Controller: A PID controller calculates the error between the Setpoint and the Process Variable.

   • error = desired_angle - actual_angle

   • However, the PID gains are set to KP=0.8, KI=0.0, KD=0.0. This means the Integral and Derivative terms are disabled, making it a Proportional-only (P) Controller.

   • The output control signal u is calculated simply as: u = KP * error.

5. Actuation: The control signal u (normalized from -1.0 to 1.0) is fed into the Steer() function, which drives the steering motor left or right with a corresponding PWM value to reduce the error to zero.

6. Reverse Logic: When a negative throttle command is active (reversing), the target steering angle is inverted (e.g., 15° becomes -15°). This correctly orients the vehicle, as turning the wheel right in reverse makes the rear of the car move right.

Throttle Control (Open-Loop)

Unlike the steering, the throttle control is a simpler, open-loop system.

1. Goal: Set the power of the drive motors based on the TH command.

2. Logic:

   • If the TH value is positive, MoveForward() is called.

   • If the TH value is negative, MoveBackward() is called.

   • The TH value (from 0.0 to 1.0) is mapped directly to a PWM duty cycle (from 0 to 255).

3. No Feedback: The system does not use the wheel encoder data to verify the vehicle's actual speed. It only "sets and forgets" the motor power. For example, a TH 0.5 command will result in the same motor power whether the vehicle is going uphill, downhill, or on flat ground, and the actual speed will vary.