Pracitical Path Following Control

Clipchamp.6.mp4

What is Practical Path Following Control ?

This package is a path-following control package developed under the NEDO ITS energy project [1]. It achieves high-accuracy trajectory tracking for automated driving. The system consists of a path-following controller that computes the target yaw rate based on lateral error and heading error, and a yaw-rate controller that drives the actual yaw rate to follow the target. By employing gain-scheduled control based on vehicle speed, the desired control performance is achieved across the entire speed range.

In addition, the package performs self-diagnostics within each control module. Each node detects conditions such as communication delays, lane departure, and wrong-way driving, and outputs warnings and errors externally. By leveraging this diagnostic information, users will be able to build highly reliable automated-driving modules on their own.

Block diagram

Version

Primary modules: path_following, yawrate_controller
Others: demo utilities (simulation, static planning, visualization)
Tested environment: Ubuntu 22.04 + ROS 2 Humble
Legacy release: v0.1.0

Quick start (scripts)

Build from the repository root:
bash build_setup.sh

Run the end-to-end demo pipeline:
bash run_path_following.sh

Notes:

• build_setup.sh sources ROS Humble, wipes install/ build/ log/, and builds with -DCMAKE_BUILD_TYPE=Release.
• run_path_following.sh opens multiple terminals via gnome-terminal and launches: simulator → yaw-rate controller → path-following → RViz visualizer → static CSV path publisher.
• Edit run_path_following.sh and set path_file:=/ABS/PATH/to/trajectory.csv to an absolute CSV path on your machine.
• If you do not have gnome-terminal, either install it or run each launch command manually (see “Manual launches”).

Control method

Each algorithm implements the methods described in the cited references; please consult the respective publications for details.

Path following control method
The PATH FOLLOWING control provides both fixed-gain and gain-scheduled options, and the control mode can be switched via YAML.

• Yoshida, J., Sugimachi, T., Fukao (2011). Autonomous driving of a truck based on path following control. Nippon Kikai Gakkai Ronbunshu, C Hen/Transactions of the Japan Society of Mechanical Engineers, Part C .

Yaw-rate control method
The yaw-rate control offers: (1) two-degree-of-freedom control using a KINEMATIC VEHICLE MODEL, (2) two-degree-of-freedom control using a DYNAMICS MODEL, and (3) gain-scheduled PI control. These modes can also be selected via YAML.

• KINEMATIC VEHICLE MODEL (in Japanese) Uetsuki, Y., Atsumi, T., Kurashiki, K., Sugawara, H., Oba, Y., Maeda, G., ... & Fukao, T. (2021). Automatic Steering Control of an Unmanned Transport Truck in Off-road Environments. Transactions of Society of Automotive Engineers of Japan, 52(5).

• DYNAMICS MODEL (in Japanese): Suzuki, M. (2022). Time Varying Feedforward Control and Data-Driven Control for Autonomous Driving Vehicle. IEEJ Transactions on Electronics, Information and Systems, 142(12), 1313-1320.

• gain-scheduled PI Yahagi, S., & Suzuki, M. (2023). Controller parameter tuning for gain‐scheduled vehicle yaw‐rate control: Virtual reference feedback tuning approach. Electronics Letters, 59(6), e12764.

Note (dynamics mode): Due to the equivalent bicycle model’s characteristics, low-speed instability can occur; consider switching to kinematic or gain-scheduled mode at low speeds and apply appropriate steering/curvature limits.

Diagnostics

As part of the diagnostics, the path-following controller detects lane departure, heading anomalies, communication delays on each signal, and timestamp-synchronization faults. The yaw-rate controller detects excessive yaw-rate error, communication delays on each signal, and timestamp-synchronization faults.

path_following

Diagnostic output topic:

• /geoauto/control/diag (diagnostic_msgs/DiagnosticArray)

Warnings/Errors:

• Lane departure — WARN/ERROR

• Wrong direction — WARN/ERROR

• Time sync delay (target vs odometry) — WARN/ERROR

• Odometry time delay (now vs odometry) — WARN/ERROR

• Target time delay (now vs target) — WARN/ERROR

yawrate_controller

Diagnostic output topic:

• /geoauto/control/diag_yaw (diagnostic_msgs/DiagnosticArray)

Warnings/Errors:

• Large yawrate error — WARN/ERROR

• Time sync delay (reference vs odometry) — WARN/ERROR

• Yawrate delay (now vs odometry) — WARN/ERROR

• Reference delay (now vs reference) — WARN/ERROR

Package

Repository layout

• build_setup.sh — build helper
• run_path_following.sh — multi-terminal demo launcher
• src/config/trajectory_info.csv — sample CSV for target trajectory
• src/geoauto_msgs/ — custom messages
• src/path_following/ — primary: path-following controller
• src/yawrate_controller/ — primary: yaw-rate (steering) controller (launch: launch/control_run.py, params: param/param.yaml)
• src/demo_static_planning/ — demo: CSV path loader → target publisher (launch: launch/static_run.py)
• src/demo_vehicle_path_viz/ — demo: RViz visualization (launch: launch/viz_run.py, default config: config/vehicle_path_viz.rviz)
• src/demo_bicycle_magic_formula_sim/ — demo: vehicle dynamics simulator (launch: launch/control_run.py, params: param/param.yaml)

Important: For any CSV inputs and RViz config files, use absolute paths. Relative paths may fail. Demo launch defaults also point to absolute example paths—replace them accordingly.

---

Interfaces

Topics (as wired in code/launch)

Path following (path_following)

• Subscribe: geoauto/localization/odometry (nav_msgs/Odometry)
• Subscribe: geoauto/planning/odom_pose (geoauto_msgs/OdomTargetPos)
• Publish: geoauto/control/target_yawrate (geometry_msgs/TwistStamped)
• Publish: geoauto/control/diag (diagnostic_msgs/DiagnosticArray)

Yaw-rate controller (yawrate_controller)

• Subscribe: /geoauto/localization/odometry (nav_msgs/Odometry)
• Subscribe: /geoauto/control/target_yawrate (geometry_msgs/TwistStamped)
• Publish: geoauto/control/steering (ackermann_msgs/AckermannDriveStamped)
• Publish: geoauto/control/diag_yaw (diagnostic_msgs/DiagnosticArray)

demo_static_planning (src/demo_static_planning/launch/static_run.py → node params) (demo: demo_static_planning)**

• Publish: /geoauto/planning/trajectory (geoauto_msgs/OdomTargetPos)
• Publish: /geoauto/planning/velocity (geometry_msgs/TwistStamped)

demo_vehicle_path_viz (demo: demo_vehicle_path_viz)

• Subscribe: /geoauto/localization/odometry (nav_msgs/Odometry)

demo_bicycle_magic_formula_sim (demo: demo_bicycle_magic_formula_sim)

• Subscribe: /geoauto/control/steering
• Publish: /geoauto/localization/odometry

---

Parameters (selected)

Path following (src/path_following/param/param.yaml)

• Fixed gain and timing: k2, k3, time_period, target_velocity, wheel_base
• Gain scheduling: use_gainscheduler, speed_bp, K2_tab, K3_tab
• Saturation: max_yawrate
• Diagnostics thresholds: lateral_error_warn_th, lateral_error_error_th, heading_error_warn_th, heading_error_error_th, timestamp_error_warn_th, timestamp_error_error_th, odom_time_warn_valid_th, odom_time_error_valid_th, pathinfo_time_warn_valid_th, pathinfo_time_error_valid_th

Yaw-rate controller (src/yawrate_controller/param/param.yaml)

• Mode: controller_mode (0 = kinematic, 1 = dynamic, 2 = gain-scheduled PI)
• PI and timing: kp, ki, dt, time_period, wheelbase, time_constant
• Limits: max_steering, max_steering_velocity
• Gain scheduling (vs speed): wP (c0,c1,c2), wI (c0,c1,c2)
• Diagnostics thresholds: yawrate_error_warn_th, yawrate_error_error_th, timestamp_error_warn_th, timestamp_error_error_th, yawrate_delay_warn_th, yawrate_delay_error_th, reference_delay_warn_th, reference_delay_error_th

demo_bicycle_magic_formula_sim (src/demo_bicycle_magic_formula_sim/param/param.yaml)

• Frames: frame_id, child_frame_id
• Topics: cmd_topic, odom_topic
• Vehicle/tire: mass, Iz, lf, lr, Magic-Formula params (mu_f, mu_r, Bf, Cf, Ef, Br, Cr, Er)
• Longitudinal: tau_v, a_max, a_min, drag_coeff, etc.
• Limits/initial state: steer_max, cmd_timeout, vx_eps, x0, y0, yaw0, vx0, vy0, r0

demo_vehicle_path_viz (src/demo_vehicle_path_viz/launch/viz_run.py → node params)

• csv_path (absolute path required), odom_topic, frame_id, theta_in_degrees, appearance and history parameters
• RViz config: launch arg rviz_cfg (absolute path if overriding; default provided in the package: config/vehicle_path_viz.rviz).

demo_static_planning (src/demo_static_planning/launch/static_run.py → node params)

• path_file (absolute path required), theta_in_degrees, frame_id, child_frame_id
• Default CSV path in launch: /home/motoya/control_ws/ROS2_Path_following_control/src/config/trajectory_info.csv

---

CSV format

Expected columns: x, y, theta, curvature, target_velocity
theta can be degrees or radians (controlled by the theta_in_degrees parameter).
Always pass CSV file paths as absolute paths.

---

Versioning

Legacy: v0.1.0 (old)
Current: this repository (Ubuntu 22.04 / ROS 2 Humble)

---

License

See each package.xml. Apply a top-level license file if desired.

Reference

• [1] - ITS Energy report, 2013: https://www.nedo.go.jp/content/100095912.pdf