Skip to content

Commit

Permalink
feat(control_data_collecting_tool): add safety measures and rosbag re…
Browse files Browse the repository at this point in the history 
…cording and loading (#134)

* add safety measures

Signed-off-by: Yoshihiro Kogure <[email protected]>

* add rosbag2 recording and loading functionalities

Signed-off-by: Yoshihiro Kogure <[email protected]>

* slight modifications

Signed-off-by: Yoshihiro Kogure <[email protected]>

* revert unncessary chnages

Signed-off-by: Yoshihiro Kogure <[email protected]>

* add a flag to determin whether to load rosbag2 data or not

Signed-off-by: Yoshihiro Kogure <[email protected]>

* record rosbag only if operation_mode is 3

Signed-off-by: Yoshihiro Kogure <[email protected]>

* add some comments and made minor adjustments

Signed-off-by: Yoshihiro Kogure <[email protected]>

* style(pre-commit): autofix

* Update control_data_collecting_tool/scripts/data_collecting_rosbag_record.py

Signed-off-by: Kosuke Takeuchi  <[email protected]>

* fix typos

Signed-off-by: Yoshihiro Kogure <[email protected]>

* subscribe control_mode

Signed-off-by: Yoshihiro Kogure <[email protected]>

* fix typo

Signed-off-by: Yoshihiro Kogure <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Yoshihiro Kogure <[email protected]>
Signed-off-by: Kosuke Takeuchi  <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Kosuke Takeuchi <[email protected]>
  • Loading branch information
@YoshihiroKogure @pre-commit-ci @kosuke55
3 people authored Oct 23, 2024
1 parent af16f8c commit dd938c8
Show file tree
Hide file tree
Showing 11 changed files with 1,010 additions and 487 deletions.
4 changes: 4 additions & 0 deletions control_data_collecting_tool/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,10 @@ install(PROGRAMS
scripts/data_collecting_pure_pursuit_trajectory_follower.py
scripts/data_collecting_trajectory_publisher.py
scripts/data_collecting_plotter.py
scripts/data_collecting_rosbag_record.py
scripts/data_collecting_data_counter.py
scripts/data_collecting_base_node.py
scripts/rosbag_play.py
DESTINATION lib/${PROJECT_NAME}
)

Expand Down
97 changes: 41 additions & 56 deletions control_data_collecting_tool/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,12 @@ This package provides tools for automatically collecting data using pure pursuit
ros2 launch control_data_collecting_tool control_data_collecting_tool.launch.py
```

- Control data collecting tool automatically records topics included in `config/topics.yaml` when the above command is executed.
The topics will be saved in rosbag2 format in the directory where the above command is executed.

- The data from `/localization/kinematic_state` and `/localization/acceleration` located in the directory (rosbag2 format) where the command is executed will be automatically loaded and reflected in the data count for these topics.
(If `LOAD_ROSBAG2_FILES` in `config/param.yaml` is set to `false`, the data is noe loaded.)

5. Add visualization in rviz:

- `/data_collecting_area`
Expand All @@ -53,31 +59,28 @@ This package provides tools for automatically collecting data using pure pursuit
> [!NOTE]
> You cannot change the data collecting area while driving.
7. start recording rosbag data. For example, run the following command:

```bash
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
```

8. Click the `LOCAL` button on `OperationMode` in `AutowareStatePanel`.
7. Click the `LOCAL` button on `OperationMode` in `AutowareStatePanel`.

<img src="resource/push_LOCAL.png" width="480">

Then, data collecting starts.

<img src="resource/demo.gif" width="480">

9. If you want to stop data collecting automatic driving, run the following command
8. If you want to stop data collecting automatic driving, run the following command

```bash
ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: true" --once
```

10. If you want to restart data collecting automatic driving, run the following command
> [!NOTE]
> When the car crosses the green boundary line, a similar stopping procedure will be automatically triggered.
```bash
ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: false" --once
```
9. If you want to restart data collecting automatic driving, run the following command

```bash
ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: false" --once
```

## Change Courses

Expand All @@ -95,22 +98,25 @@ You can change the course by selecting `COURSE_NAME` in `config/param.yaml` from

## Parameter

ROS 2 params in `/data_collecting_trajectory_publisher` node:
ROS 2 params which are common in all nodes:

| Name | Type | Description | Default value |
| :--------------------------------------- | :------- | :-------------------------------------------------------------------------------------------------- | :------------- |
| `LOAD_ROSBAG2_FILES` | `bool` | Flag that determines whether to load rosbag2 data or not | True |
| `COURSE_NAME` | `string` | Course name [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`] | `eight_course` |
| `NUM_BINS_V` | `int` | Number of bins of velocity in heatmap | 10 |
| `NUM_BINS_STEER` | `int` | Number of bins of steer in heatmap | 10 |
| `NUM_BINS_ACCELERATION` | `int` | Number of bins of acceleration in heatmap | 10 |
| `NUM_BINS_A` | `int` | Number of bins of acceleration in heatmap | 10 |
| `V_MIN` | `double` | Minimum velocity in heatmap [m/s] | 0.0 |
| `V_MAX` | `double` | Maximum velocity in heatmap [m/s] | 11.5 |
| `STEER_MIN` | `double` | Minimum steer in heatmap [rad] | -1.0 |
| `STEER_MAX` | `double` | Maximum steer in heatmap [rad] | 1.0 |
| `A_MIN` | `double` | Minimum acceleration in heatmap [m/ss] | -1.0 |
| `A_MAX` | `double` | Maximum acceleration in heatmap [m/ss] | 1.0 |
| `wheel_base` | `double` | Wheel base [m] | 2.79 |
| `acc_kp` | `double` | Accel command proportional gain | 1.0 |
| `COLLECTING_DATA_V_MIN` | `double` | Minimum velocity for data collection [m/s] | 0.0 |
| `COLLECTING_DATA_V_MAX` | `double` | Maximum velocity for data collection [m/s] | 11.5 |
| `COLLECTING_DATA_A_MIN` | `double` | Minimum velocity for data collection [m/ss] | 1.0 |
| `COLLECTING_DATA_A_MAX` | `double` | Maximum velocity for data collection [m/ss] | -1.0 |
| `max_lateral_accel` | `double` | Max lateral acceleration limit [m/ss] | 0.5 |
| `lateral_error_threshold` | `double` | Lateral error threshold where applying velocity limit [m/s] | 5.0 |
| `yaw_error_threshold` | `double` | Yaw error threshold where applying velocity limit [rad] | 0.50 |
Expand All @@ -120,43 +126,22 @@ ROS 2 params in `/data_collecting_trajectory_publisher` node:
| `longitudinal_velocity_noise_amp` | `double` | Target longitudinal velocity additional sine noise amplitude [m/s] | 0.01 |
| `longitudinal_velocity_noise_min_period` | `double` | Target longitudinal velocity additional sine noise minimum period [s] | 5.0 |
| `longitudinal_velocity_noise_max_period` | `double` | Target longitudinal velocity additional sine noise maximum period [s] | 20.0 |

ROS 2 params in `/data_collecting_pure_pursuit_trajectory_follower` node:

| Name | Type | Description | Default value |
| :--------------------------------------- | :------- | :------------------------------------------------------------- | :------------ |
| `pure_pursuit_type` | `string` | Pure pursuit type (`naive` or `linearized` steer control law ) | `linearized` |
| `wheel_base` | `double` | Wheel base [m] | 2.79 |
| `acc_kp` | `double` | Accel command proportional gain | 1.0 |
| `lookahead_time` | `double` | Pure pursuit lookahead time [s] | 2.0 |
| `min_lookahead` | `double` | Pure pursuit minimum lookahead length [m] | 2.0 |
| `linearized_pure_pursuit_steer_kp_param` | `double` | Linearized pure pursuit steering P gain parameter | 2.0 |
| `linearized_pure_pursuit_steer_kd_param` | `double` | Linearized pure pursuit steering D gain parameter | 2.0 |
| `stop_acc` | `double` | Accel command for stopping data collecting driving [m/ss] | -2.0 |
| `stop_jerk_lim` | `double` | Jerk limit for stopping data collecting driving [m/sss] | 1.0 |
| `lon_acc_lim` | `double` | Longitudinal acceleration limit [m/ss] | 5.0 |
| `lon_jerk_lim` | `double` | Longitudinal jerk limit [m/sss] | 5.0 |
| `steer_lim` | `double` | Steering angle limit [rad] | 1.0 |
| `steer_rate_lim` | `double` | Steering angle rate limit [rad/s] | 1.0 |
| `acc_noise_amp` | `double` | Accel command additional sine noise amplitude [m/ss] | 0.01 |
| `acc_noise_min_period` | `double` | Accel command additional sine noise minimum period [s] | 5.0 |
| `acc_noise_max_period` | `double` | Accel command additional sine noise maximum period [s] | 20.0 |
| `steer_noise_amp` | `double` | Steer command additional sine noise amplitude [rad] | 0.01 |
| `steer_noise_max_period` | `double` | Steer command additional sine noise maximum period [s] | 5.0 |
| `steer_noise_min_period` | `double` | Steer command additional sine noise minimum period [s] | 20.0 |

ROS 2 params in `/data_collecting_plotter` node:

| Name | Type | Description | Default value |
| :---------------------- | :------- | :-------------------------------------------------------------------------------------------------- | :------------- |
| `COURSE_NAME` | `string` | Course name [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`] | `eight_course` |
| `NUM_BINS_V` | `int` | Number of bins of velocity in heatmap | 10 |
| `NUM_BINS_STEER` | `int` | Number of bins of steer in heatmap | 10 |
| `NUM_BINS_ACCELERATION` | `int` | Number of bins of acceleration in heatmap | 10 |
| `V_MIN` | `double` | Minimum velocity in heatmap [m/s] | 0.0 |
| `V_MAX` | `double` | Maximum velocity in heatmap [m/s] | 11.5 |
| `STEER_MIN` | `double` | Minimum steer in heatmap [rad] | -1.0 |
| `STEER_MAX` | `double` | Maximum steer in heatmap [rad] | 1.0 |
| `A_MIN` | `double` | Minimum acceleration in heatmap [m/ss] | -1.0 |
| `A_MAX` | `double` | Maximum acceleration in heatmap [m/ss] | 1.0 |
| `wheel_base` | `double` | Wheel base [m] | 2.79 |
| `pure_pursuit_type` | `string` | Pure pursuit type (`naive` or `linearized` steer control law ) | `linearized` |
| `wheel_base` | `double` | Wheel base [m] | 2.79 |
| `acc_kp` | `double` | Accel command proportional gain | 1.0 |
| `lookahead_time` | `double` | Pure pursuit lookahead time [s] | 2.0 |
| `min_lookahead` | `double` | Pure pursuit minimum lookahead length [m] | 2.0 |
| `linearized_pure_pursuit_steer_kp_param` | `double` | Linearized pure pursuit steering P gain parameter | 2.0 |
| `linearized_pure_pursuit_steer_kd_param` | `double` | Linearized pure pursuit steering D gain parameter | 2.0 |
| `stop_acc` | `double` | Accel command for stopping data collecting driving [m/ss] | -2.0 |
| `stop_jerk_lim` | `double` | Jerk limit for stopping data collecting driving [m/sss] | 5.0 |
| `lon_acc_lim` | `double` | Longitudinal acceleration limit [m/ss] | 5.0 |
| `lon_jerk_lim` | `double` | Longitudinal jerk limit [m/sss] | 5.0 |
| `steer_lim` | `double` | Steering angle limit [rad] | 1.0 |
| `steer_rate_lim` | `double` | Steering angle rate limit [rad/s] | 1.0 |
| `acc_noise_amp` | `double` | Accel command additional sine noise amplitude [m/ss] | 0.01 |
| `acc_noise_min_period` | `double` | Accel command additional sine noise minimum period [s] | 5.0 |
| `acc_noise_max_period` | `double` | Accel command additional sine noise maximum period [s] | 20.0 |
| `steer_noise_amp` | `double` | Steer command additional sine noise amplitude [rad] | 0.01 |
| `steer_noise_max_period` | `double` | Steer command additional sine noise maximum period [s] | 5.0 |
| `steer_noise_min_period` | `double` | Steer command additional sine noise minimum period [s] | 20.0 |
28 changes: 9 additions & 19 deletions control_data_collecting_tool/config/param.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,7 @@
data_collecting_plotter:
/**:
ros__parameters:
NUM_BINS_V: 10
NUM_BINS_STEER: 10
NUM_BINS_A: 10
V_MIN: 0.0
V_MAX: 11.5
STEER_MIN: -1.0
STEER_MAX: 1.0
A_MIN: -1.0
A_MAX: 1.0

wheel_base: 2.79
LOAD_ROSBAG2_FILES: true

data_collecting_trajectory_publisher:
ros__parameters:
COURSE_NAME: eight_course
# COURSE_NAME: u_shaped_return
# COURSE_NAME: straight_line_positive
Expand All @@ -29,8 +17,11 @@ data_collecting_trajectory_publisher:
A_MIN: -1.0
A_MAX: 1.0

wheel_base: 2.79
acc_kp: 1.0
COLLECTING_DATA_V_MIN: 0.0
COLLECTING_DATA_V_MAX: 11.5
COLLECTING_DATA_A_MIN: -1.0
COLLECTING_DATA_A_MAX: 1.0

max_lateral_accel: 0.5
lateral_error_threshold: 2.0
yaw_error_threshold: 0.50
Expand All @@ -41,17 +32,16 @@ data_collecting_trajectory_publisher:
longitudinal_velocity_noise_min_period: 5.0
longitudinal_velocity_noise_max_period: 20.0

data_collecting_pure_pursuit_trajectory_follower:
ros__parameters:
pure_pursuit_type: linearized
# pure_pursuit_type: naive
wheel_base: 2.79
acc_kp: 1.0
lookahead_time: 2.0
min_lookahead: 2.0
linearized_pure_pursuit_steer_kp_param: 2.0
linearized_pure_pursuit_steer_kd_param: 2.0
stop_acc: -2.0
stop_jerk_lim: 2.0
stop_jerk_lim: 5.0
lon_acc_lim: 2.0
lon_jerk_lim: 5.0
steer_lim: 1.0
Expand Down
10 changes: 10 additions & 0 deletions control_data_collecting_tool/config/topics.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
topics:
- /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
11 changes: 11 additions & 0 deletions control_data_collecting_tool/launch/control_data_collecting_tool.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,17 @@ def generate_launch_description():
name="data_collecting_plotter",
parameters=[param_file_path],
),
Node(
package="control_data_collecting_tool",
executable="data_collecting_rosbag_record.py",
name="data_collecting_rosbag_record",
),
Node(
package="control_data_collecting_tool",
executable="data_collecting_data_counter.py",
name="data_collecting_data_counter",
parameters=[param_file_path],
),
]
)

Expand Down
Loading

0 comments on commit dd938c8

Please sign in to comment.