diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 86cdedc6c..16980de9e 100644 --- a/control_data_collecting_tool/README.md +++ b/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 +## 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. + + + + 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. + + + + + + - `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. + + + + + + - `boundary` + + The `boundary` part is used to connect the `common tangent` and the `circumscribing_circle` sections. + + + + + +- #### 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. + + + ## 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` diff --git a/control_data_collecting_tool/config/common_param.yaml b/control_data_collecting_tool/config/common_param.yaml index 1a578f0be..3e5c12c4c 100644 --- a/control_data_collecting_tool/config/common_param.yaml +++ b/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 diff --git a/control_data_collecting_tool/config/course_param/reversal_loop_circle_param.yaml b/control_data_collecting_tool/config/course_param/reversal_loop_circle_param.yaml index 096215298..55939b394 100644 --- a/control_data_collecting_tool/config/course_param/reversal_loop_circle_param.yaml +++ b/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 diff --git a/control_data_collecting_tool/resource/boundary.png b/control_data_collecting_tool/resource/boundary.png new file mode 100644 index 000000000..8cc3b88ea Binary files /dev/null and b/control_data_collecting_tool/resource/boundary.png differ diff --git a/control_data_collecting_tool/resource/boundary_counter_clockwise.png b/control_data_collecting_tool/resource/boundary_counter_clockwise.png new file mode 100644 index 000000000..dbdd21782 Binary files /dev/null and b/control_data_collecting_tool/resource/boundary_counter_clockwise.png differ diff --git a/control_data_collecting_tool/resource/circumscribing_circle.png b/control_data_collecting_tool/resource/circumscribing_circle.png new file mode 100644 index 000000000..2c1996f92 Binary files /dev/null and b/control_data_collecting_tool/resource/circumscribing_circle.png differ diff --git a/control_data_collecting_tool/resource/circumscribing_circle_counter_clockwise.png b/control_data_collecting_tool/resource/circumscribing_circle_counter_clockwise.png new file mode 100644 index 000000000..be54aaf91 Binary files /dev/null and b/control_data_collecting_tool/resource/circumscribing_circle_counter_clockwise.png differ diff --git a/control_data_collecting_tool/resource/common_tangent.png b/control_data_collecting_tool/resource/common_tangent.png new file mode 100644 index 000000000..77af0f3f1 Binary files /dev/null and b/control_data_collecting_tool/resource/common_tangent.png differ diff --git a/control_data_collecting_tool/resource/common_tangent_counter_clockwise.png b/control_data_collecting_tool/resource/common_tangent_counter_clockwise.png new file mode 100644 index 000000000..b1208b25e Binary files /dev/null and b/control_data_collecting_tool/resource/common_tangent_counter_clockwise.png differ diff --git a/control_data_collecting_tool/resource/looking_ahead.png b/control_data_collecting_tool/resource/looking_ahead.png new file mode 100644 index 000000000..10d0dcad3 Binary files /dev/null and b/control_data_collecting_tool/resource/looking_ahead.png differ diff --git a/control_data_collecting_tool/resource/whole_trajectory.png b/control_data_collecting_tool/resource/whole_trajectory.png new file mode 100644 index 000000000..5830d522b Binary files /dev/null and b/control_data_collecting_tool/resource/whole_trajectory.png differ diff --git a/control_data_collecting_tool/scripts/courses/reversal_loop_circle.py b/control_data_collecting_tool/scripts/courses/reversal_loop_circle.py index a053c02f0..108df22dc 100644 --- a/control_data_collecting_tool/scripts/courses/reversal_loop_circle.py +++ b/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 diff --git a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py index e7f91c323..c3c2d41b9 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/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]" ),