feat(control_data_collecting_tool): improve README and a slight modif…

…ication to the data collection logic (#179)

* Improve README

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

* Modify data collction logic

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

* Final adjustment

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

* Spelling correction

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

* pre-commit run

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

* style(pre-commit): autofix

---------

Signed-off-by: Yoshihiro Kogure <yoshihiro.kogure@proxima-ai-tech.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

control_data_collecting_tool/README.md

@@ -181,6 +181,98 @@ You can create an original mask to specify the data collection range for the hea
 
    <img src="resource/original.png" width="480">
 
+## Trajectory generation and data collection logic
+
+- ### Data collection logic common to all courses
+
+  In `control_data_collection_tool`, all courses collect velocity and acceleration data in a similar manner.
+
+  By appropriately adjusting the `target_velocity` provided to the pure pursuit algorithm, speed and acceleration data are efficiently collected. Data collection consists of four phases: selection of target speed and acceleration, acceleration phase, constant speed phase, the deceleration phase. A general method for collecting speed and acceleration data is described below, though it does not strictly adhere to the outlined steps.
+
+  1. Selection of target speed and acceleration
+
+     In the speed-acceleration heatmap, the speed and acceleration with fewer data points are set as the target speed and acceleration, which are then defined here as `target_velocity_on_section` and `target_acceleration_on_section`.
+
+  2. Acceleration phase
+     The vehicle accelerates by setting `target_velocity` as follows until its speed exceeds `target_velocity_on_section`.
+
+     ```Python3
+       # sine_curve is derived from appropriate amplitude and period, defined separately
+       target_velocity = current_velocity + abs(target_acceleration_on_section) / acc_kp + sine_curve
+     ```
+
+     `current_velocity` is a current velocity of vehicle and `acc_kp` accel command proportional gain in pure pursuit algorithm. `sine_curve` is a sine wave added to partially mitigate situations where the vehicle fails to achieve the target acceleration.
+
+  3. Constant speed phase
+     When the vehicle reaches `target_velocity_on_section`, `target_velocity` is defined as follows to allow the vehicle to run around the target speed for a certain period of time.
+
+     ```Python3
+       # sine_curve is derived from appropriate amplitude and period, defined separately
+       target_velocity = target_velocity_on_section + sine_curve + sine_curve
+     ```
+
+  4. Deceleration phase
+     In the deceleration phase, similar to the acceleration phase, `target_velocity` is defined as follows to ensure the vehicle decelerates.
+
+     ```Python3
+       # sine_curve is derived from appropriate amplitude and period, defined separately
+       target_velocity = current_velocity - abs(target_acceleration_on_section) / acc_kp + sine_curve
+     ```
+
+     After decelerating to a sufficiently low speed, return to step i.
+
+- ### Trajectory generation and data collection logic specific to `reversal_loop_circle`
+
+  In the `reversal_loop_circle` course, sections are sequentially added to the course while collecting various data on speed, acceleration, and steering angle.
+
+- #### Trajectory generation logic for `reversal_loop_circle`
+
+  The `reversal_loop_circle` aims to generate a trajectory with the largest possible radius of curvature within a radius `trajectory_radius`, allowing data collection in a confined area without significantly reducing speed.
+
+  The `reversal_loop_circle` is primarily generated by connecting the following three components, `common tangent`, `circumscribing_circle` and `boundary` as shown in the following picture.
+
+    <img src="resource/whole_trajectory.png" width="480">
+
+  All the components listed below are available in both clockwise and counterclockwise versions. The rationale for having two versions is to ensure data collection for both right-hand drive and left-hand drive configurations.
+
+  - `common tangent`
+
+    The `common tangent` is generated by drawing a common tangent to two inscribed circles. In this section, a sine curve is added in the normal direction to generate a trajectory for collecting data with larger steering angles.
+    The amplitude of the sine curve is determined based on the desired steering angle data.
+
+      <img src="resource/common_tangent.png" width="480">
+
+      <img src="resource/common_tangent_counter_clockwise.png" width="480">
+
+  - `circumscribing_circle`
+
+    The `circumscribing_circle` part is created by drawing the common circumscribed circle of the circles.
+    This section is generated to achieve nearly straight movement within the outer circle while minimizing the increase in curvature.
+
+      <img src="resource/circumscribing_circle.png" width="480">
+
+      <img src="resource/circumscribing_circle_counter_clockwise.png" width="480">
+
+  - `boundary`
+
+    The `boundary` part is used to connect the `common tangent` and the `circumscribing_circle` sections.
+
+      <img src="resource/boundary.png" width="480">
+
+      <img src="resource/boundary_counter_clockwise.png" width="480">
+
+- #### Velocity and steering angle data collection logic for `reversal_loop_circle`
+
+  Speed and steering angle data are gathered in the `common tangent` section of the trajectory. The common tangent section is particularly effective for collecting steering angle data because a trajectory with minimal data is intentionally created by adding a sine wave of suitable amplitude to the curvature.
+
+  The following two steps are taken to obtain steering angle data.
+
+  1. Starting from the ego vehicle's current position, the system examines a segment of the trajectory ahead, covering a distance defined by looking_ahead_distance, to identify the point of maximum curvature.
+
+  2. This maximum curvature determines the steering angle the vehicle will use. The vehicle then adjusts its speed toward the speed associated with the sparsest steering angle data.
+
+    <img src="resource/looking_ahead.png" width="480">
+
 ## Parameter
 
 There are parameters that are common to all trajectories and parameters that are specific to each trajectory.
@@ -204,7 +296,7 @@ ROS 2 parameters which are common in all trajectories (`/config/common_param.yam
 | `STEER_MAX`                              | `double` | Maximum steer in heatmap [rad]                                                                                                            | 0.6                    |
 | `A_MIN`                                  | `double` | Minimum acceleration in heatmap [m/s^2]                                                                                                   | -1.0                   |
 | `A_MAX`                                  | `double` | Maximum acceleration in heatmap [m/s^2]                                                                                                   | 1.0                    |
-| `max_lateral_accel`                      | `double` | Max lateral acceleration limit [m/s^2]                                                                                                    | 2.00                   |
+| `max_lateral_accel`                      | `double` | Max lateral acceleration limit [m/s^2]                                                                                                    | 2.70                   |
 | `ABS_STEER_RATE_MIN`                     | `double` | Minimum absolute value of steer rate in heatmap [rad/s]                                                                                   | 0.0                    |
 | `ABS_STEER_RATE_MAX`                     | `double` | Maximum absolute value of steer rate in heatmap [rad/s]                                                                                   | 0.3                    |
 | `JERK_MIN`                               | `double` | Minimum jerk in heatmap [m/s^3]                                                                                                           | -0.5                   |
@@ -280,7 +372,7 @@ Each trajectory has specific ROS 2 parameters.
 | :-------------------- | :------- | :---------------------------------------------------------------------------------- | :------------ |
 | `trajectory_radius`   | `double` | Radius of the circle where trajectories are generated [m]                           | 35.0          |
 | `enclosing_radius`    | `double` | Radius of the circle enclosing the generated trajectories [m]                       | 40.0          |
-| `look_ahead_distance` | `double` | The distance referenced ahead of the vehicle for collecting steering angle data [m] | 15.0          |
+| `look_ahead_distance` | `double` | The distance referenced ahead of the vehicle for collecting steering angle data [m] | 35.0          |
 
 - `COURSE_NAME: along_road`
 

control_data_collecting_tool/config/common_param.yaml

@@ -30,7 +30,7 @@
     VEL_STEER_THRESHOLD: 20
     VEL_ABS_STEER_RATE_THRESHOLD: 20
 
-    max_lateral_accel: 2.00
+    max_lateral_accel: 2.70
     lateral_error_threshold: 1.50
     yaw_error_threshold: 0.75
     velocity_limit_by_tracking_error: 1.0

control_data_collecting_tool/config/course_param/reversal_loop_circle_param.yaml

@@ -3,7 +3,7 @@
     # Course Specific Parameters
     trajectory_radius: 35.0
     enclosing_radius: 40.0
-    look_ahead_distance: 15.0
+    look_ahead_distance: 35.0
 
     # Data Collection Range
     COLLECTING_DATA_V_MIN: 0.5

control_data_collecting_tool/scripts/courses/reversal_loop_circle.py

@@ -752,7 +752,7 @@ def declare_reversal_loop_circle_params(node):
 
     node.declare_parameter(
         "look_ahead_distance",
-        15.0,
+        35.0,
         ParameterDescriptor(
             description="The distance referenced ahead of the vehicle for collecting steering angle data"
         ),
@@ -1030,74 +1030,81 @@ def get_target_velocity(
             self.vehicle_phase = "acceleration"
             self.updated_target_velocity = True
 
+            self.alpha = 0.5 + np.random.randint(0, 2) * 0.5
+
         acc_kp_of_pure_pursuit = self.params.acc_kp  # Proportional gain for acceleration control
-        T = 10.0  # Period of the sine wave used to modulate velocity
+        # Period should be parameterized
+        T = 5.0  # Period of the sine wave used to modulate velocity
         sine = np.sin(2 * np.pi * current_time / T)  # Sine wave for smooth velocity modulation
 
+        target_acc = 0.0
         # Handle acceleration phase
         if self.vehicle_phase == "acceleration":
             if current_vel < self.target_vel_on_segmentation - 1.0 * abs(
                 self.target_acc_on_segmentation
             ):
                 # Increase velocity with a maximum allowable acceleration
-                target_vel = current_vel + self.params.a_max / acc_kp_of_pure_pursuit * (
-                    1.25 + 0.50 * sine
+                target_acc = (
+                    self.alpha * self.params.a_max / acc_kp_of_pure_pursuit * (0.4 + 0.6 * sine)
                 )
             else:
                 # Increase velocity with a absolute target acceleration
-                target_vel = current_vel + abs(
-                    self.target_acc_on_segmentation
-                ) / acc_kp_of_pure_pursuit * (1.25 + 0.50 * sine)
+                target_acc = (
+                    abs(self.target_acc_on_segmentation) / acc_kp_of_pure_pursuit + 0.1 * sine
+                )
 
             # Transition to "constant speed" phase once the target velocity is reached
             if current_vel > self.target_vel_on_segmentation:
                 self.vehicle_phase = "constant_speed"
                 self.const_velocity_start_time = current_time
 
-        # Handle constant speed phase
-        if self.vehicle_phase == "constant_speed":
-            # Modulate velocity around the target with a sine wave
-            target_vel = self.target_vel_on_segmentation + 2.0 * (-0.5 + 1.0 * sine)
-
-            # Transition to "deceleration" phase after a fixed duration
-            if current_time - self.const_velocity_start_time > T:
-                self.vehicle_phase = "deceleration"
-
         # Handle deceleration phase
         if self.vehicle_phase == "deceleration":
             if current_vel < self.target_vel_on_segmentation - 1.0 * abs(
                 self.target_acc_on_segmentation
             ):
                 # Decrease velocity with a maximum deceleration
-                target_vel = current_vel - self.params.a_max / acc_kp_of_pure_pursuit * (
-                    1.25 + 0.50 * sine
+                target_acc = (
+                    -self.alpha * self.params.a_max / acc_kp_of_pure_pursuit * (0.4 + 0.6 * sine)
                 )
             else:
                 # Decrease velocity with a absolute target acceleration
-                target_vel = current_vel - abs(
-                    self.target_acc_on_segmentation
-                ) / acc_kp_of_pure_pursuit * (1.25 + 0.50 * sine)
+                target_acc = (
+                    -abs(self.target_acc_on_segmentation) / acc_kp_of_pure_pursuit + 0.1 * sine
+                )
 
             # Reset velocity update flag when deceleration is complete
-            if (
-                current_vel
-                < self.target_vel_on_segmentation
-                - 1.0 * abs(self.target_acc_on_segmentation) / acc_kp_of_pure_pursuit
-            ):
+            if current_vel < self.target_vel_on_segmentation / 4.0:
                 self.updated_target_velocity = False
 
         # Maintain a smoothed velocity by averaging recent values
+        # 10.0 should be parameterized
+        target_vel = current_vel + target_acc + 10.0 * (target_acc - current_acc)
+
+        # Handle constant speed phase
+        if self.vehicle_phase == "constant_speed":
+            # Modulate velocity around the target with a sine wave
+            target_vel = (
+                self.target_vel_on_segmentation
+                + 2.0
+                * np.sin(2 * np.pi * current_time / 5.0)
+                * np.sin(2 * np.pi * current_time / 10.0)
+                - 2.0
+            )
+            if current_time - self.const_velocity_start_time > 20.0:
+                self.vehicle_phase = "deceleration"
+
         self.vel_hist.append(target_vel)
         target_vel = np.mean(self.vel_hist)
 
         # Special handling for trajectory direction changes
         if self.trajectory_list[2].in_direction is not self.trajectory_list[2].out_direction:
             # Set a fixed target velocity during direction transitions
-            target_vel = 3.0 + 1.0 * sine
+            target_vel = 2.0 + 2.0 * sine
 
         # Adjust velocity based on trajectory curvature and lateral acceleration constraints
-        if (self.trajectory_list[1].in_direction is not self.trajectory_list[1].out_direction) or (
-            self.trajectory_list[2].in_direction is not self.trajectory_list[2].out_direction
+        if (self.trajectory_list[0].in_direction is not self.trajectory_list[0].out_direction) or (
+            self.trajectory_list[1].in_direction is not self.trajectory_list[1].out_direction
         ):
             max_curvature_on_segment = 1e-9  # Initialize to a small value to avoid division by zero
             max_lateral_vel_on_segment = 1e9  # Initialize to a large value as a placeholder
@@ -1140,10 +1147,7 @@ def get_target_velocity(
             max_vel_from_lateral_acc_on_segment = np.sqrt(
                 self.params.max_lateral_accel / max_curvature_on_segment
             )
-            target_vel = np.min([target_vel_ + 0.5 * sine, max_vel_from_lateral_acc_on_segment])
-
-        # Ensure the target velocity remains above a minimum threshold
-        target_vel = np.max([target_vel, 0.5])
+            target_vel = np.min([target_vel_ + 1.0 * sine**2, max_vel_from_lateral_acc_on_segment])
 
         return target_vel
 

control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py

@@ -72,7 +72,7 @@ def __init__(self):
 
         self.declare_parameter(
             "lateral_error_threshold",
-            2.0,
+            2.70,
             ParameterDescriptor(
                 description="Lateral error threshold where applying velocity limit [m/s]"
             ),