feat(data_collecting_tool): add multiple courses (#113)

* add multiple courses

Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

* modify steer calculation

Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

* reformat file

Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

* add parameter file
Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

* modify parameter files and readme
Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

* modify readme
Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

* modification via pre-commit
Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

* modify params

Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

* Revert "modify params"

This reverts commit 369fb12.

Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

* remove unnecesary comments

Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

* modify params

Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

---------

Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>

control_data_collecting_tool/CMakeLists.txt

@@ -10,6 +10,11 @@ install(PROGRAMS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(
+  DIRECTORY config/
+  DESTINATION share/${PROJECT_NAME}/config
+)
+
 find_package(Qt5 REQUIRED Core Widgets)
 set(QT_LIBRARIES Qt5::Widgets)
 

control_data_collecting_tool/README.md

@@ -79,21 +79,47 @@ This package provides tools for automatically collecting data using pure pursuit
 ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: false" --once
 ```
 
+## Change Courses
+
+You can change the course by selecting `COURSE_NAME` in `config/param.yaml` from [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`].
+
+- `COURSE_NAME: eight_course`
+  <img src="resource/figure_eight.png" width="480">
+
+- `COURSE_NAME: u_shaped_return`
+  <img src="resource/u_shaped.png" width="480">
+
+- `COURSE_NAME: straight_line_positive` or `COURSE_NAME: straight_line_negative`
+  ( Both "straight_line_positive" and "straight_line_negative" represent straight line courses, but the direction of travel of the course is reversed.)
+  <img src="resource/straight_line.png" width="480">
+
 ## Parameter
 
 ROS 2 params in `/data_collecting_trajectory_publisher` node:
 
-| Name                                     | Type     | Description                                                           | Default value |
-| :--------------------------------------- | :------- | :-------------------------------------------------------------------- | :------------ |
-| `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.75          |
-| `velocity_limit_by_tracking_error`       | `double` | Velocity limit applied when tracking error exceeds threshold [m/s]    | 1.0           |
-| `mov_ave_window`                         | `int`    | Moving average smoothing window size                                  | 50            |
-| `target_longitudinal_velocity`           | `double` | Target longitudinal velocity [m/s]                                    | 6.0           |
-| `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          |
+| 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           |
+| `acc_kp`                                 | `double` | Accel command proportional gain                                                                     | 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           |
+| `velocity_limit_by_tracking_error`       | `double` | Velocity limit applied when tracking error exceeds threshold [m/s]                                  | 1.0            |
+| `mov_ave_window`                         | `int`    | Moving average smoothing window size                                                                | 100            |
+| `target_longitudinal_velocity`           | `double` | Target longitudinal velocity [m/s]                                                                  | 6.0            |
+| `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:
 

control_data_collecting_tool/config/param.yaml

@@ -0,0 +1,51 @@
+common_parameters:
+  ros__parameters:
+    wheel_base: 2.79
+    acc_kp: 1.0
+
+data_collecting_trajectory_publisher:
+  ros__parameters:
+    COURSE_NAME: eight_course
+    # COURSE_NAME: u_shaped_return
+    # COURSE_NAME: straight_line_positive
+    # COURSE_NAME: straight_line_negative
+
+    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
+
+    max_lateral_accel: 0.5
+    lateral_error_threshold: 2.0
+    yaw_error_threshold: 0.50
+    velocity_limit_by_tracking_error: 1.0
+    mov_ave_window: 100
+    target_longitudinal_velocity: 6.0
+    longitudinal_velocity_noise_amp: 0.01
+    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
+    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
+    lon_acc_lim: 2.0
+    lon_jerk_lim: 5.0
+    steer_lim: 1.0
+    steer_rate_lim: 1.0
+    acc_noise_amp: 0.01
+    acc_noise_min_period: 5.0
+    acc_noise_max_period: 20.0
+    steer_noise_amp: 0.01
+    steer_noise_min_period: 5.0
+    steer_noise_max_period: 20.0

control_data_collecting_tool/launch/control_data_collecting_tool.launch.py

@@ -14,23 +14,30 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
 import launch
 from launch import LaunchService
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
+    package_share_directory = get_package_share_directory("control_data_collecting_tool")
+    param_file_path = os.path.join(package_share_directory, "config", "param.yaml")
     return launch.LaunchDescription(
         [
             Node(
                 package="control_data_collecting_tool",
                 executable="data_collecting_pure_pursuit_trajectory_follower.py",
                 name="data_collecting_pure_pursuit_trajectory_follower",
+                parameters=[param_file_path],
             ),
             Node(
                 package="control_data_collecting_tool",
                 executable="data_collecting_trajectory_publisher.py",
                 name="data_collecting_trajectory_publisher",
+                parameters=[param_file_path],
             ),
         ]
     )

control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py

@@ -131,7 +131,7 @@ def __init__(self):
 
         self.declare_parameter(
             "acc_noise_amp",
-            0.00,
+            0.01,
             ParameterDescriptor(description="Accel cmd additional sine noise amplitude [m/ss]"),
         )
 
@@ -149,7 +149,7 @@ def __init__(self):
 
         self.declare_parameter(
             "steer_noise_amp",
-            0.00,
+            0.01,
             ParameterDescriptor(description="Steer cmd additional sine noise amplitude [rad]"),
         )