|
| 1 | +# Control data collecting tool |
| 2 | + |
| 3 | +This package provides tools for automatically collecting data using pure pursuit control within a specified rectangular area. |
| 4 | + |
| 5 | +<img src="resource/demo.gif" width="800"> |
| 6 | + |
| 7 | +## Overview |
| 8 | + |
| 9 | +- This package aims to collect a dataset consisting of control inputs (i.e. `control_cmd`) and observation variables (i.e. `kinematic_state`, `steering_status`, etc). |
| 10 | +- The collected dataset can be used as training dataset for learning-based controllers, including [smart_mpc](https://github.com/autowarefoundation/autoware.universe/tree/f30c0350861d020ad26a45806ab1334895122fab/control/smart_mpc_trajectory_follower). |
| 11 | +- The data collecting approach is as follows: |
| 12 | + - Setting a figure-eight target trajectory within the specified rectangular area. |
| 13 | + - Following the trajectory using a pure pursuit control law. |
| 14 | + - Adding noises to the trajectory and the control command for data diversity, improving the prediction accuracy of learning model. |
| 15 | + |
| 16 | +## How to use |
| 17 | + |
| 18 | +1. Launch Autoware. |
| 19 | + |
| 20 | + ```bash |
| 21 | + ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit |
| 22 | + ``` |
| 23 | + |
| 24 | +2. Set an initial pose, see [here](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/#2-set-an-initial-pose-for-the-ego-vehicle). |
| 25 | + |
| 26 | +3. Add `DataCollectingAreaSelectionTool` rviz plugin. |
| 27 | + |
| 28 | + <img src="resource/add_rviz_plugin.png" width="480"> |
| 29 | + |
| 30 | +4. Launch control_data_collecting_tool. |
| 31 | + |
| 32 | + ```bash |
| 33 | + ros2 launch control_data_collecting_tool control_data_collecting_tool.launch.py |
| 34 | + ``` |
| 35 | + |
| 36 | +5. Add visualization in rviz: |
| 37 | + |
| 38 | + - `/data_collecting_area` |
| 39 | + - Type: Polygon |
| 40 | + - `/data_collecting_trajectory_marker_array` |
| 41 | + - Type: MarkerArray |
| 42 | + - `/data_collecting_lookahead_marker_array` |
| 43 | + - Type: MarkerArray |
| 44 | + |
| 45 | +6. Select `DataCollectingAreaSelectionTool` plugin. |
| 46 | + |
| 47 | + <img src="resource/select_tool.png" width="480"> |
| 48 | + |
| 49 | + Highlight the data collecting area by dragging the mouse over it. |
| 50 | + |
| 51 | + <img src="resource/select_area.gif" width="480"> |
| 52 | + |
| 53 | + > [!NOTE] |
| 54 | + > You cannot change the data collecting area while driving. |
| 55 | +
|
| 56 | +7. start recording rosbag data. For example, run the following command: |
| 57 | + |
| 58 | + ```bash |
| 59 | + ros2 bag record /localization/kinematic_state /localization/acceleration /vehicle/status/steering_status /sensing/imu/imu_data /system/operation_mode/state /vehicle/status/control_mode /external/selected/control_cmd /external/selected/gear_cmd /data_collecting_trajectory |
| 60 | + ``` |
| 61 | + |
| 62 | +8. Click the `LOCAL` button on `OperationMode` in `AutowareStatePanel`. |
| 63 | + |
| 64 | + <img src="resource/push_LOCAL.png" width="480"> |
| 65 | + |
| 66 | + Then, data collecting starts. |
| 67 | + |
| 68 | + <img src="resource/demo.gif" width="480"> |
| 69 | + |
| 70 | +9. If you want to stop data collecting automatic driving, run the following command |
| 71 | + |
| 72 | + ```bash |
| 73 | + ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: true" --once |
| 74 | + ``` |
| 75 | + |
| 76 | +10. If you want to restart data collecting automatic driving, run the following command |
| 77 | + |
| 78 | +```bash |
| 79 | +ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: false" --once |
| 80 | +``` |
| 81 | + |
| 82 | +## Parameter |
| 83 | + |
| 84 | +ROS 2 params in `/data_collecting_trajectory_publisher` node: |
| 85 | + |
| 86 | +| Name | Type | Description | Default value | |
| 87 | +| :--------------------------------------- | :------- | :-------------------------------------------------------------------- | :------------ | |
| 88 | +| `max_lateral_accel` | `double` | Max lateral acceleration limit [m/ss] | 2.94 | |
| 89 | +| `lateral_error_threshold` | `double` | Lateral error threshold where applying velocity limit [m/s] | 5.0 | |
| 90 | +| `yaw_error_threshold` | `double` | Yaw error threshold where applying velocity limit [rad] | 0.5 | |
| 91 | +| `velocity_limit_by_tracking_error` | `double` | Velocity limit applied when tracking error exceeds threshold [m/s] | 2.0 | |
| 92 | +| `mov_ave_window` | `int` | Moving average smoothing window size | 50 | |
| 93 | +| `target_longitudinal_velocity` | `double` | Target longitudinal velocity [m/s] | 6.0 | |
| 94 | +| `longitudinal_velocity_noise_amp` | `double` | Target longitudinal velocity additional sine noise amplitude [m/s] | 0.1 | |
| 95 | +| `longitudinal_velocity_noise_min_period` | `double` | Target longitudinal velocity additional sine noise minimum period [s] | 5.0 | |
| 96 | +| `longitudinal_velocity_noise_max_period` | `double` | Target longitudinal velocity additional sine noise maximum period [s] | 20.0 | |
| 97 | + |
| 98 | +ROS 2 params in `/data_collecting_pure_pursuit_trajectory_follower` node: |
| 99 | + |
| 100 | +| Name | Type | Description | Default value | |
| 101 | +| :--------------------------------------- | :------- | :------------------------------------------------------------- | :------------ | |
| 102 | +| `pure_pursuit_type` | `string` | Pure pursuit type (`naive` or `linearized` steer control law ) | `linearized` | |
| 103 | +| `wheel_base` | `double` | Wheel base [m] | 2.79 | |
| 104 | +| `acc_kp` | `double` | Accel command proportional gain | 0.5 | |
| 105 | +| `lookahead_time` | `double` | Pure pursuit lookahead time [s] | 1.5 | |
| 106 | +| `min_lookahead` | `double` | Pure pursuit minimum lookahead length [m] | 3.0 | |
| 107 | +| `linearized_pure_pursuit_steer_kp_param` | `double` | Linearized pure pursuit steering P gain parameter | 2.0 | |
| 108 | +| `linearized_pure_pursuit_steer_kd_param` | `double` | Linearized pure pursuit steering D gain parameter | 2.0 | |
| 109 | +| `stop_acc` | `double` | Accel command for stopping data collecting driving [m/ss] | -2.0 | |
| 110 | +| `stop_jerk_lim` | `double` | Jerk limit for stopping data collecting driving [m/sss] | 1.0 | |
| 111 | +| `lon_acc_lim` | `double` | Longitudinal acceleration limit [m/ss] | 5.0 | |
| 112 | +| `lon_jerk_lim` | `double` | Longitudinal jerk limit [m/sss] | 5.0 | |
| 113 | +| `steer_lim` | `double` | Steering angle limit [rad] | 1.0 | |
| 114 | +| `steer_rate_lim` | `double` | Steering angle rate limit [rad/s] | 1.0 | |
| 115 | +| `acc_noise_amp` | `double` | Accel command additional sine noise amplitude [m/ss] | 0.01 | |
| 116 | +| `acc_noise_min_period` | `double` | Accel command additional sine noise minimum period [s] | 5.0 | |
| 117 | +| `acc_noise_max_period` | `double` | Accel command additional sine noise maximum period [s] | 20.0 | |
| 118 | +| `steer_noise_amp` | `double` | Steer command additional sine noise amplitude [rad] | 0.01 | |
| 119 | +| `steer_noise_max_period` | `double` | Steer command additional sine noise maximum period [s] | 5.0 | |
| 120 | +| `steer_noise_min_period` | `double` | Steer command additional sine noise minimum period [s] | 20.0 | |
0 commit comments