Merge pull request #3 from KariControl/feat_gain_scheduled

Feat gain scheduled

Author: KariControl

Readme.md

@@ -1,15 +1,203 @@
-動作確認環境：
-OS:ubuntu 22.04
-、ROS version: ROS2 humble
+# Pracitical Path Following Control
 
-このqiitaの記事で書いたものをROS2 humbleで実装してみました：
-※ ヨーレート制御の部分だけ少しだけシンプルにしています。
-https://qiita.com/KariControl/items/78862ae6b45fcfe91126
 
-デモ版のソフトでは
 
- source run_path_following.sh
 
-を実行すると，下記のように一定曲率の目標パスに対して軌道追従する制御シミュレーンが実行されます。デモ版ソフトのため、目標パス計算もpath following controlのノード内で実行しています。
-![image](https://github.com/user-attachments/assets/990da0b5-a167-4a28-b64e-37fd6dce813a)
 
+
+https://github.com/user-attachments/assets/3c09d58a-7c07-4a6a-a513-a9c25786458e
+
+
+
+
+
+# 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.
+
+<img width="849" height="688" alt="image" src="https://github.com/user-attachments/assets/87611efa-20a5-41ba-bec6-90ec68f8229f" />
+
+# Block diagram
+
+<img width="2023" height="773" alt="image" src="https://github.com/user-attachments/assets/83d0edad-ee25-44a8-9ee7-f01b4d016727" />
+
+# 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  

build_setup.sh

@@ -0,0 +1,4 @@
+source /opt/ros/humble/setup.bash
+rm -r install/ build/ log/
+colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+#colcon build --symlink-install