StudentMobilityCompetition-Planning-Control

This repository contains my personal work for the Planning & High-Level Control Team in the Student Mobility Competition (Team K.A.I., Konkuk University).

It implements a full ROS2 stack that plans global paths from GPS data, generates local paths from traffic cones, and tracks them using Pure Pursuit, enabling our custom-built EV to drive autonomously on cone-marked tracks.

Watch the Demo

• On-road Test
• First-person (POV) driving video
• Debug Visualization(RViz+Plotjuggler)

---

📁 Project Structure

Planning/
├── cone_labeling_k/ # Potential Field-based Planning
│   └── cone_labeling_k
│       ├── launch
│       │   └── cone_labeling.launch.py
│       ├── potential_field.py  # latest version(fixed several problems)
│       └── potential_field.py.BAK  # old version
│
├── cones_no_color/ # Local path generation based on traffic cones
│   ├── msg/  # custon msg that describes detected cones from Perception
│   ├── scripts/
│   │   ├── cone_safe_zone.py
│   │   ├── reference_path_planning.py
│   │   └── visualize_cones.py
│   └── launch/ …
│
├── gps_global_planner/ # Global path generation using GPS·RTK
│   ├── data/ # CSVs containing Longitude/Latitude, UTM Coordinates, Covariances
│   ├── launch/
│   │   ├── gps_global_planner_launch.py
│   ├── scripts/
│   │   ├── auto_place_cones.py
│   │   ├── cone_roi_publisher.py
│   │   ├── course_csv_creator.py
│   │   ├── global_yaw_estimator_node.py
│   │   └── publish_global_cones.py
│   └── src/
│       ├── gps_to_local_cartesian.cpp
│       ├── local_cartesian_path_publisher.cpp
│       ├── status_colored_path_publisher.cpp
│       └── vehicle_tf_broadcaster.cpp
│
├── reference_path_classifier/ # Classifies Cones into Left and Rigth Sides(Using Global Path)
│   ├── launch/
│   │   └── classify_cones_by_side.launch.py
│   ├── scripts/
│   │   └── classify_cones_by_side.py
│   └── src/
│       └── classify_cones_by_side.cpp
│
├── speed_planner/ # Calculates Desired Speed for Each Waypoints based on Curvature of the Path
│   ├── launch/
│   │   └── simple_speed_planner.launch.py
│   ├── config/
│   │   └── simple_speed_planner.yaml
│   └── src/  # Publishes speed and rpm command
│       └── simple_speed_planner.cpp
│
├── pure_pursuit/ # Path Tracking Algorithm - 3 different versions
│   ├── config/ # paremeter tuning files
│   │   ├── adaptive_pp.yaml
│   │   ├── pure_pursuit_dynamic.yaml
│   │   └── pure_pursuit_static.yaml
│   ├── launch/
│   │   ├── adaptive_pure_pursuit.launch.py
│   │   ├── pure_pursuit_dynamic.launch.py
│   │   └── pure_pursuit_static.launch.py
│   └── src/  # 3 different methods nodes
│       ├── pure_pursuit_adaptive.cpp
│       ├── pure_pursuit_dynamic.cpp
│       └── pure_pursuit_static.cpp
│
└ How To Play.md # Contains How to Run My Packages

---

Ideal rqt graph

Flow Summary (Concise):

• Cone perception → Local path: /cone/* → /cones_color_subscriber → /local_planned_path, /cones_marker_array (RViz shows /cones/markers).
• AEB decision: /cones_color_subscriber → /AEB_Determination_Node → /aeb_roi, /aeb.
• Curvature/Speed planning: /local_planned_path → /path_sampler → /sp/rep_curvature → /speed_zone_planner → /cmd/speed, /cmd/rpm.
• Path tracking control: /pure_pursuit_adaptive ⟵ /local_planned_path, /current_speed → /cmd/steer (+ /pure_pursuit/* debug topics).
• HUD/Visualization: /hud_overlay_node ⟵ /cmd/, /odometry/filtered, /pure_pursuit/ → /hud/overlay_text, /hud/aeb_overlay, /current_kmh; /car_marker_publisher ⟵ /cmd/steer → /car_marker.

---

Key Features

Package	Main Functions	Language
cones_no_color	• Generates an optimal reference path from cone locations • Provides RViz visualization nodes	Python
gps_global_planner	• Converts RTK-GPS logs to CSV and nav_msgs/Path • ENU ↔ Local Cartesian conversion • Colors the global path by state and speed	C++, Python
reference_path_classifier	• Classifies cones on the left and right sides relative to the vehicle	Python
pure_pursuit	• Tracks the local path via Pure Pursuit (static/dynamic/adaptive look-ahead) • Subscribes nav_msgs/Path (/local_planned_path) and /current_speed; publishes /cmd/steer and RViz debug markers	C++

---

Usage

• Refer to How To Play.md

Build (ROS2 Humble)

cd [workspace path]
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

• Use --symlink-install for immediate reflection of source code changes in the install directory.
• Use -DCMAKE_BUILD_TYPE=Release for optimized execution performance.

Example Execution

# 1) Generate and publish the GPS-based global path
ros2 launch gps_global_planner gps_global_planner_launch.py

# 2) Classify left-right cones using Global Planned Path
ros2 launch reference_path_classifier classify_cones_by_side.launch.py

# 3) Generate a local path based on detected cones
ros2 run cones_no_color reference_path_planning.py

# 4) Generate and publish the desired speed and RPM based on curvature of the path
ros2 launch speed_planner simple_speed_planner.launch.py

# 5) Track the local path using (Adaptive) Pure Pursuit Algorithm
ros2 launch pure_pursuit adaptive_pure_pursuit.launch.py

---

Demo Video (Main)

1. Right/Left Classification (Frenet + K-D Tree)	2. Delaunay Triangulation
3. Local Speed Planning	4. Path Tracking (Pure Pursuit)

📁 View More (4 Additional Steps)

5. Global Path Planning	6. Global Cones and ROI (Sim)
7. Interpolation & Sampling	8. Obstacle Radius & Safe Zone

---

Several Attempts for Better Planning and Tracking

Adaptive Pure Pursuit

This GIF visualizes the Adaptive Pure Pursuit algorithm, based on the paper “Accurate Path Tracking by Adjusting Look-Ahead Point”. The look-ahead distance dynamically adjusts depending on vehicle speed and path curvature, enabling more accurate and stable path tracking compared to standard Pure Pursuit.

• Pink Marker: Look-Ahead Point of Adaptive Pure Pursuit
• Orange Arrow: Vehicle Heading Direction based on Adaptive Pure Pursuit

In curves, the look-ahead distance is reduced to minimize path deviation, while on straights it is increased for smoother tracking, improving trajectory stability under diverse driving conditions.

---

Potential Field-Based Local Path Planning

This GIF demonstrates local path planning using a Potential Field method. The algorithm generates a field around all detected cones and identifies the lowest "trough" line as the optimal drivable path, marking these as Through Points (mandatory waypoints the path must pass through). The selected points are then refined with Centripetal Catmull–Rom Spline Interpolation to generate a smooth and continuous local path relative to the vehicle's base_link frame.

• Colored Grid: Potential field intensity map
• Yellow Dots: Trough Points before smoothing
• Pink Line: Final smoothed local path
• Blue / Yellow Spheres: Left and Right cones respectively

---

Greedy Graph-Guided Local Path Planning

This GIF demonstrates local path planning using a Greedy, graph-guided centerline search. The pipeline reconstructs left/right boundary lines from color-labeled cones, generates robust midpoints (Hungarian / projection / resample-align), and then selects a forward-feasible center path on a radius graph by minimizing distance with boundary, directional, and smoothness penalties under heading-change gates. The result is resampled and anchored to the vehicle's base_link frame, then published as nav_msgs/Path.

• Left/Right Lines: Reconstructed track boundaries from cones
• Green Dots: Midpoints between boundaries
• Green Line: Final fixed-length local center path
• Blue / Yellow Spheres: Left and Right cones respectively

---

ROS Bag Files (Download)

You can reproduce the reference pipeline immediately using the sample bag files below (Google Drive):

• Curve section: https://drive.google.com/drive/folders/1smqId70MtuKinn6FSc2SYskNgCNc_TE2?usp=sharing

  • Driving data(including cone topics) collected on track segments with several curves

• Straight section: https://drive.google.com/drive/folders/1rNyuKDwBvFl_HvaT3qoHntzvx84Ndx5d?usp=sharing

  • Driving data(including cone topics) collected on straight-dominant segments

• GPS+IMU–focused bags: https://drive.google.com/drive/folders/19wzDIm6VciQ7x5rul0CGEnw3SJ7GRCzu?usp=sharing

  • Data where RTK-GPS and IMU topics are the main sources (for reproducing global/local path generation and pose estimation)

---

Development Environment

Item	Version / Tool
OS	Ubuntu 22.04
ROS	ROS2 Humble
GPS RTK Module	ZED-F9P-04B-01
IMU Module	myAHRS+
Languages	Python 3.10 / C++17
Localization Stack	ros2-gnss-imu-fusion

---

References

• ROS2 Official Documentation
• MathWorks Blog: Path Planning for Formula Student Driverless Cars
• u-blox GitHub - RTKLIB, GNSS tools
• u-blox F9P Interface Description (Documentation)
• Accurate Path Tracking by Adjusting Look Ahead Point in Pure Pursuit Method

---

Contributions & Contact

• imhyeonwoo21@gmail.com
• https://www.youtube.com/@hwi1_0_0_1