diff --git a/control_data_collecting_tool/CMakeLists.txt b/control_data_collecting_tool/CMakeLists.txt index f7c14530..331366c5 100644 --- a/control_data_collecting_tool/CMakeLists.txt +++ b/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) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 5bf1ab6a..f8e7d14e 100644 --- a/control_data_collecting_tool/README.md +++ b/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` + + +- `COURSE_NAME: u_shaped_return` + + +- `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.) + + ## 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: diff --git a/control_data_collecting_tool/config/param.yaml b/control_data_collecting_tool/config/param.yaml new file mode 100644 index 00000000..cc6177f6 --- /dev/null +++ b/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 diff --git a/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py b/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py index ec3f1cc7..f1b15476 100644 --- a/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py +++ b/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], ), ] ) diff --git a/control_data_collecting_tool/resource/figure_eight.png b/control_data_collecting_tool/resource/figure_eight.png new file mode 100644 index 00000000..77aaa492 Binary files /dev/null and b/control_data_collecting_tool/resource/figure_eight.png differ diff --git a/control_data_collecting_tool/resource/straight_line.png b/control_data_collecting_tool/resource/straight_line.png new file mode 100644 index 00000000..2978a37f Binary files /dev/null and b/control_data_collecting_tool/resource/straight_line.png differ diff --git a/control_data_collecting_tool/resource/u_shaped.png b/control_data_collecting_tool/resource/u_shaped.png new file mode 100644 index 00000000..d14dcba8 Binary files /dev/null and b/control_data_collecting_tool/resource/u_shaped.png differ diff --git a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py index 202f1178..c142744d 100755 --- a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py +++ b/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]"), ) 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 b49e7338..00af7fe8 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -14,6 +14,9 @@ # See the License for the specific language governing permissions and # limitations under the License. + +import threading + from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint from geometry_msgs.msg import AccelWithCovarianceStamped @@ -22,21 +25,23 @@ import matplotlib.pyplot as plt from nav_msgs.msg import Odometry import numpy as np -from numpy import arctan +from numpy import arctan2 from numpy import cos from numpy import pi from numpy import sin from rcl_interfaces.msg import ParameterDescriptor import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from scipy.spatial.transform import Rotation as R import seaborn as sns from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray -debug_matplotlib_plot_flag = True +debug_matplotlib_plot_flag = False +data_counts_matplotlib_plot_flag = True Differential_Smoothing_Flag = True -USE_CURVATURE_RADIUS_FLAG = True +USE_CURVATURE_RADIUS_FLAG = False def smooth_bounding(upper: np.ndarray, threshold: np.ndarray, x: np.ndarray): @@ -54,13 +59,14 @@ def getYaw(orientation_xyzw): return R.from_quat(orientation_xyzw.reshape(-1, 4)).as_euler("xyz")[:, 2] -def get_trajectory_points( - long_side_length: float, short_side_length: float, step: float, total_distance: float +def get_eight_course_trajectory_points( + long_side_length: float, + short_side_length: float, + step: float, ): a = short_side_length b = long_side_length - # Boundary points between circular and linear trajectory C = [-(b / 2 - (1.0 - np.sqrt(3) / 2) * a), -a / 2] D = [(b / 2 - (1.0 - np.sqrt(3) / 2) * a), -a / 2] @@ -71,7 +77,7 @@ def get_trajectory_points( (b / 2 + (1.0 - np.sqrt(3) / 2) * a) ** 2 + (a / 2) ** 2 ) # half length of the linear trajectory AD = 2 * OB - θB = arctan( + θB = np.arctan( a / 2 / (b / 2 + (1.0 - np.sqrt(3) / 2) * a) ) # Angle that OB makes with respect to x-axis BD = 2 * np.pi * R / 6 # the length of arc BD @@ -106,8 +112,8 @@ def get_trajectory_points( if OB <= t and t <= OB + BD: t1 = t - OB t1_rad = t1 / R - x[i] = OR[0] + R * cos(np.pi / 6 - t1_rad) - y[i] = OR[1] + R * sin(np.pi / 6 - t1_rad) + x[i] = OR[0] + R * np.cos(np.pi / 6 - t1_rad) + y[i] = OR[1] + R * np.sin(np.pi / 6 - t1_rad) yaw[i] = -t1_rad curve[i] = 1 / R parts[i] = "right_circle" @@ -126,8 +132,8 @@ def get_trajectory_points( if OB + BD + AD <= t and t <= OB + BD + AD + AC: t3 = t - (OB + BD + AD) t3_rad = t3 / R - x[i] = OL[0] - R * cos(-np.pi / 6 + t3_rad) - y[i] = OL[1] - R * sin(-np.pi / 6 + t3_rad) + x[i] = OL[0] - R * np.cos(-np.pi / 6 + t3_rad) + y[i] = OL[1] - R * np.sin(-np.pi / 6 + t3_rad) yaw[i] = np.pi + t3_rad curve[i] = 1 / R @@ -158,51 +164,187 @@ def get_trajectory_points( return np.array([x, y]).T, yaw, curve[:i_end], parts, achievement_rates +def get_straight_line_course_trajectory_points( + long_side_length: float, short_side_length: float, step: float, COURSE_NAME +): + b = long_side_length + + total_distance = b + t_array = np.arange(start=0.0, stop=total_distance, step=step).astype("float") + + if COURSE_NAME == "straight_line_positive": + yaw = np.zeros(len(t_array)) + parts = ["linear_positive" for _ in range(len(t_array.copy()))] + x = np.linspace(-total_distance / 2, total_distance / 2, len(t_array)) + y = np.zeros(len(t_array)) + + elif COURSE_NAME == "straight_line_negative": + yaw = pi * np.ones(len(t_array)) + parts = ["linear_negative" for _ in range(len(t_array.copy()))] + x = np.linspace(total_distance / 2, -total_distance / 2, len(t_array)) + y = np.zeros(len(t_array)) + + curve = 1e-9 * np.ones(len(t_array)) + achievement_rates = np.linspace(0.0, 1.0, len(t_array)) + + if USE_CURVATURE_RADIUS_FLAG: + return np.vstack((x, y)).T, yaw, 1 / curve, parts, achievement_rates + else: + return np.vstack((x, y)).T, yaw, curve, parts, achievement_rates + + +def get_u_shaped_return_course_trajectory_points( + long_side_length: float, short_side_length: float, step: float +): + a = short_side_length + b = long_side_length + + # Boundary points between circular and linear trajectory + A = [-(b - a) / 2, a / 2] + B = [(b - a) / 2, a / 2] + C = [-(b - a) / 2, -a / 2] + D = [(b - a) / 2, -a / 2] + + R = a / 2 # radius of the circle + OL = [-(b - a) / 2, 0] # center of the left circle + OR = [(b - a) / 2, 0] # center of the right circle + + AB = b - a + arc_BD = pi * R + DC = b - a + arc_CA = pi * R + + total_distance = 2 * AB + 2 * np.pi * R + + t_array = np.arange(start=0.0, stop=total_distance, step=step).astype("float") + x = [0.0 for i in range(len(t_array.copy()))] + y = [0.0 for i in range(len(t_array.copy()))] + yaw = t_array.copy() + curve = t_array.copy() + achievement_rates = t_array.copy() + parts = ["part" for _ in range(len(t_array.copy()))] + i_end = t_array.shape[0] + + for i, t in enumerate(t_array): + if t > AB + arc_BD + DC + arc_CA: + i_end = i + break + + if 0 <= t and t <= AB: + section_rate = t / AB + x[i] = section_rate * B[0] + (1 - section_rate) * A[0] + y[i] = section_rate * B[1] + (1 - section_rate) * A[1] + yaw[i] = 0.0 + curve[i] = 1e-10 + parts[i] = "linear_positive" + achievement_rates[i] = section_rate + + if AB <= t and t <= AB + arc_BD: + section_rate = (t - AB) / arc_BD + x[i] = OR[0] + R * cos(pi / 2 - pi * section_rate) + y[i] = OR[1] + R * sin(pi / 2 - pi * section_rate) + yaw[i] = pi / 2 - pi * section_rate - pi / 2 + curve[i] = 1.0 / R + parts[i] = "right_circle" + achievement_rates[i] = section_rate + + if AB + arc_BD <= t and t <= AB + arc_BD + DC: + section_rate = (t - AB - arc_BD) / DC + x[i] = section_rate * C[0] + (1 - section_rate) * D[0] + y[i] = section_rate * C[1] + (1 - section_rate) * D[1] + yaw[i] = pi + curve[i] = 1e-10 + parts[i] = "linear_negative" + achievement_rates[i] = section_rate + + if AB + arc_BD + DC <= t and t <= AB + arc_BD + DC + arc_CA: + section_rate = (t - AB - arc_BD - DC) / arc_CA + x[i] = OL[0] + R * cos(3 * pi / 2 - pi * section_rate) + y[i] = OL[1] + R * sin(3 * pi / 2 - pi * section_rate) + yaw[i] = 3 * pi / 2 - pi * section_rate - pi / 2 + curve[i] = 1.0 / R + parts[i] = "left_circle" + achievement_rates[i] = section_rate + + # drop rest + x = x[:i_end] + y = y[:i_end] + yaw = yaw[:i_end] + curve = curve[:i_end] + parts = parts[:i_end] + achievement_rates = achievement_rates[:i_end] + + if USE_CURVATURE_RADIUS_FLAG: + return np.array([x, y]).T, yaw, 1 / curve[:i_end], parts, achievement_rates + else: + return np.array([x, y]).T, yaw, curve[:i_end], parts, achievement_rates + + class DataCollectingTrajectoryPublisher(Node): def __init__(self): super().__init__("data_collecting_trajectory_publisher") - # velocity and acceleration grid - # velocity and steer grid - self.num_bins = 10 - self.v_min, self.v_max = 0, 11.2 - self.steer_min, self.steer_max = -1.0, 1.0 - self.a_min, self.a_max = -1.0, 1.0 - - self.collected_data_counts_of_vel_acc = np.zeros((self.num_bins, self.num_bins)) - self.collected_data_counts_of_vel_steer = np.zeros((self.num_bins, self.num_bins)) - - self.v_bins = np.linspace(self.v_min, self.v_max, self.num_bins + 1) - self.steer_bins = np.linspace(self.steer_min, self.steer_max, self.num_bins + 1) - self.a_bins = np.linspace(self.a_min, self.a_max, self.num_bins + 1) + self.declare_parameter( + "COURSE_NAME", + "eight_course", + ParameterDescriptor( + description="Course name [eight_course, u_shaped_return, straight_line_positive, straight_line_negative]" + ), + ) - self.v_bin_centers = (self.v_bins[:-1] + self.v_bins[1:]) / 2 - self.steer_bin_centers = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 - self.a_bin_centers = (self.a_bins[:-1] + self.a_bins[1:]) / 2 + self.declare_parameter( + "NUM_BINS_V", + 10, + ParameterDescriptor(description="Number of bins of velocity in heatmap"), + ) - self.fig, self.axs = plt.subplots(4, 1, figsize=(12, 20)) - self.grid_update_time_interval = 1 - self.last_grid_update_time = None - plt.ion() + self.declare_parameter( + "NUM_BINS_STEER", + 10, + ParameterDescriptor(description="Number of bins of steer in heatmap"), + ) - self.trajectory_parts = None - self.trajectory_achievement_rates = None + self.declare_parameter( + "NUM_BINS_A", + 10, + ParameterDescriptor(description="Number of bins of acceleration in heatmap"), + ) - self.prev_part = "left_circle" + self.declare_parameter( + "V_MIN", + -1.0, + ParameterDescriptor(description="Maximum velocity in heatmap [m/s]"), + ) - self.acc_idx = 0 - self.vel_idx = 0 + self.declare_parameter( + "V_MAX", + 1.0, + ParameterDescriptor(description="Minimum steer in heatmap [m/s]"), + ) - self.target_acc_on_line = 0.0 - self.target_vel_on_line = 0.0 + self.declare_parameter( + "STEER_MIN", + -1.0, + ParameterDescriptor(description="Maximum steer in heatmap [rad]"), + ) - self.on_line_vel_flag = True + self.declare_parameter( + "STEER_MAX", + 1.0, + ParameterDescriptor(description="Maximum steer in heatmap [rad]"), + ) - self.previous_target_vel = 2.0 - self.deceleration_rate = 0.6 + self.declare_parameter( + "A_MIN", + -1.0, + ParameterDescriptor(description="Minimum acceleration in heatmap [m/ss]"), + ) - self.vel_hist = np.zeros(50) - self.acc_hist = np.zeros(50) + self.declare_parameter( + "A_MAX", + 1.0, + ParameterDescriptor(description="Maximum acceleration in heatmap [m/ss]"), + ) self.declare_parameter( "wheel_base", @@ -224,7 +366,7 @@ def __init__(self): self.declare_parameter( "lateral_error_threshold", - 5.0, + 2.0, ParameterDescriptor( description="Lateral error threshold where applying velocity limit [m/s]" ), @@ -232,7 +374,7 @@ def __init__(self): self.declare_parameter( "yaw_error_threshold", - 0.75, + 0.50, ParameterDescriptor( description="Yaw error threshold where applying velocity limit [rad]" ), @@ -248,7 +390,7 @@ def __init__(self): self.declare_parameter( "mov_ave_window", - 200, + 100, ParameterDescriptor(description="Moving average smoothing window size"), ) @@ -260,7 +402,7 @@ def __init__(self): self.declare_parameter( "longitudinal_velocity_noise_amp", - 0.0, + 0.01, ParameterDescriptor( description="Target longitudinal velocity additional sine noise amplitude [m/s]" ), @@ -316,10 +458,94 @@ def __init__(self): ) self.sub_data_collecting_area_ + # set course name + self.COURSE_NAME = self.get_parameter("COURSE_NAME").value + + # velocity and acceleration grid + # velocity and steer grid + self.num_bins_v = self.get_parameter("NUM_BINS_V").get_parameter_value().integer_value + self.num_bins_steer = ( + self.get_parameter("NUM_BINS_STEER").get_parameter_value().integer_value + ) + self.num_bins_a = self.get_parameter("NUM_BINS_A").get_parameter_value().integer_value + self.v_min, self.v_max = ( + self.get_parameter("V_MIN").get_parameter_value().double_value, + self.get_parameter("V_MAX").get_parameter_value().double_value, + ) + self.steer_min, self.steer_max = ( + self.get_parameter("STEER_MIN").get_parameter_value().double_value, + self.get_parameter("STEER_MAX").get_parameter_value().double_value, + ) + self.a_min, self.a_max = ( + self.get_parameter("A_MIN").get_parameter_value().double_value, + self.get_parameter("A_MAX").get_parameter_value().double_value, + ) + + self.collected_data_counts_of_vel_acc = np.zeros((self.num_bins_v, self.num_bins_a)) + self.collected_data_counts_of_vel_steer = np.zeros((self.num_bins_v, self.num_bins_steer)) + + self.v_bins = np.linspace(self.v_min, self.v_max, self.num_bins_v + 1) + self.steer_bins = np.linspace(self.steer_min, self.steer_max, self.num_bins_steer + 1) + self.a_bins = np.linspace(self.a_min, self.a_max, self.num_bins_a + 1) + + self.v_bin_centers = (self.v_bins[:-1] + self.v_bins[1:]) / 2 + self.steer_bin_centers = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 + self.a_bin_centers = (self.a_bins[:-1] + self.a_bins[1:]) / 2 + + self.trajectory_parts = None + self.trajectory_achievement_rates = None + + self.prev_part = "left_circle" + + self.acc_idx = 0 + self.vel_idx = 0 + + self.target_acc_on_line = 0.0 + self.target_vel_on_line = 0.0 + + self.on_line_vel_flag = True + self.deceleration_rate = 0.6 + + self.vel_hist = np.zeros(200) + self.acc_hist = np.zeros(200) + + self.vel_hist_for_plot = np.zeros(200) + self.acc_hist_for_plot = np.zeros(200) + + self.callback_group = ReentrantCallbackGroup() + self.lock = threading.Lock() + self.timer_period_callback = 0.03 # 30ms self.traj_step = 0.1 + self.timer_traj = self.create_timer( + self.timer_period_callback, self.timer_callback_traj, callback_group=self.callback_group + ) + + if data_counts_matplotlib_plot_flag: + self.collected_data_counts_of_vel_acc_for_plot = np.zeros( + (self.num_bins_v, self.num_bins_a) + ) + self.collected_data_counts_of_vel_steer_for_plot = np.zeros( + (self.num_bins_v, self.num_bins_steer) + ) - self.timer = self.create_timer(self.timer_period_callback, self.timer_callback) + self.v_bin_centers_for_plot = (self.v_bins[:-1] + self.v_bins[1:]) / 2 + self.steer_bin_centers_for_plot = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 + self.a_bin_centers_for_plot = (self.a_bins[:-1] + self.a_bins[1:]) / 2 + + self.fig, self.axs = plt.subplots(3, 1, figsize=(12, 20)) + self.grid_update_time_interval = 2.0 + plt.ion() + + self.timer_plot = self.create_timer( + self.grid_update_time_interval, + self.timer_callback_plot, + callback_group=self.callback_group, + ) + + if debug_matplotlib_plot_flag: + self.fig_debug, self.axs_debug = plt.subplots(figsize=(12, 6)) + plt.ion() self._present_kinematic_state = None self._present_acceleration = None @@ -349,6 +575,181 @@ def onDataCollectingArea(self, msg): self._data_collecting_area_polygon = msg self.updateNominalTargetTrajectory() + def get_target_velocity(self, nearestIndex): + part = self.trajectory_parts[ + nearestIndex + ] # "left_circle", "right_circle", "linear_positive", "linear_negative" + achievement_rate = self.trajectory_achievement_rates[nearestIndex] + current_vel = self._present_kinematic_state.twist.twist.linear.x + current_acc = self._present_acceleration.accel.accel.linear.x + + acc_kp_of_pure_pursuit = self.get_parameter("acc_kp").get_parameter_value().double_value + N_V = self.num_bins_v + N_A = self.num_bins_a + + self.acc_hist[:-1] = 1.0 * self.acc_hist[1:] + self.acc_hist[-1] = current_acc + self.vel_hist[:-1] = 1.0 * self.vel_hist[1:] + self.vel_hist[-1] = current_vel + + max_lateral_accel = ( + self.get_parameter("max_lateral_accel").get_parameter_value().double_value + ) + if USE_CURVATURE_RADIUS_FLAG: + max_vel_from_lateral_acc = np.sqrt( + max_lateral_accel * self.trajectory_curvature_data[nearestIndex] + ) + else: + max_vel_from_lateral_acc = np.sqrt( + max_lateral_accel / self.trajectory_curvature_data[nearestIndex] + ) + + target_vel = min([self.v_max / N_V, max_vel_from_lateral_acc]) + + # set self.target_acc_on_line and self.target_vel_on_line after vehicle from circle part to linear part + min_data_num_margin = 5 + min_index_list = [] + if ( + (self.prev_part == "left_circle" or self.prev_part == "right_circle") + and (part == "linear_positive" or part == "linear_negative") + or ( + (part == "linear_positive" or part == "linear_negative") and achievement_rate < 0.05 + ) + ): + self.on_line_vel_flag = True + min_num_data = 1e12 + + # do not collect data when velocity and acceleration are low + exclude_idx_list = [(0, 0), (1, 0), (2, 0), (0, 1), (1, 1), (0, 2)] + # do not collect data when velocity and acceleration are high + exclude_idx_list += [ + (-1 + N_V, -1 + N_A), + (-2 + N_V, -1 + N_A), + (-3 + N_V, -1 + N_A), + (-1 + N_V, -2 + N_A), + (-2 + N_V, -2 + N_A), + (-1 + N_V, -3 + N_A), + ] + + for i in range(0, N_V): + for j in range(0, N_A): + if (i, j) not in exclude_idx_list: + if ( + min_num_data - min_data_num_margin + > self.collected_data_counts_of_vel_acc[i, j] + ): + min_num_data = self.collected_data_counts_of_vel_acc[i, j] + min_index_list.clear() + min_index_list.append((j, i)) + + elif ( + min_num_data + min_data_num_margin + > self.collected_data_counts_of_vel_acc[i, j] + ): + min_index_list.append((j, i)) + + self.acc_idx, self.vel_idx = min_index_list[np.random.randint(0, len(min_index_list))] + self.target_acc_on_line = self.a_bin_centers[self.acc_idx] + self.target_vel_on_line = self.v_bin_centers[self.vel_idx] + + if ( + self.COURSE_NAME == "u_shaped_return" + or self.COURSE_NAME == "straight_line_positive" + or self.COURSE_NAME == "straight_line_negative" + ): + if self.target_vel_on_line > self.v_max * 3.0 / 4.0: + self.deceleration_rate = 0.55 + 0.15 + elif self.target_vel_on_line > self.v_max / 2.0: + self.deceleration_rate = 0.65 + 0.25 + else: + self.deceleration_rate = 0.85 + 0.1 + + elif self.COURSE_NAME == "eight_course": + if self.target_vel_on_line > self.v_max * 3.0 / 4.0: + self.deceleration_rate = 0.55 + elif self.target_vel_on_line > self.v_max / 2.0: + self.deceleration_rate = 0.65 + else: + self.deceleration_rate = 0.85 + + # set target velocity on linear part + if part == "linear_positive" or part == "linear_negative": + if ( + current_vel > self.target_vel_on_line - self.v_max / N_V / 8.0 + and self.target_vel_on_line >= self.v_max / 2.0 + ): + self.on_line_vel_flag = False + elif ( + abs(current_vel - self.target_vel_on_line) < self.v_max / N_V / 4.0 + and self.target_vel_on_line < self.v_max / 2.0 + ): + self.on_line_vel_flag = False + + # accelerate until vehicle reaches target_vel_on_line + if 0.0 <= achievement_rate and achievement_rate < 0.45 and self.on_line_vel_flag: + target_vel = self.target_vel_on_line + + if ( + current_vel > self.target_vel_on_line - self.v_max / N_V * 0.5 + and self.target_acc_on_line > 2.0 * self.a_max / N_A + ): + target_vel = current_vel + self.target_acc_on_line / acc_kp_of_pure_pursuit + + # collect target_acceleration data when current velocity is close to target_vel_on_line + elif ( + achievement_rate < self.deceleration_rate + or self.target_vel_on_line < self.v_max / 2.0 + ): + if self.collected_data_counts_of_vel_acc[self.vel_idx, self.acc_idx] > 50: + self.acc_idx = np.argmin(self.collected_data_counts_of_vel_acc[self.vel_idx, :]) + self.target_acc_on_line = self.a_bin_centers[self.acc_idx] + + if ( + current_vel + < max( + [self.target_vel_on_line - 1.5 * self.v_max / N_V, self.v_max / N_V / 2.0] + ) + and self.target_acc_on_line < 0.0 + ): + self.acc_idx = np.argmin( + self.collected_data_counts_of_vel_acc[self.vel_idx, int(N_A / 2.0) : N_A] + ) + int(N_A / 2) + self.target_acc_on_line = self.a_bin_centers[self.acc_idx] + + elif ( + current_vel > self.target_vel_on_line + 1.5 * self.v_max / N_V + and self.target_acc_on_line > 0.0 + ): + self.acc_idx = np.argmin( + self.collected_data_counts_of_vel_acc[self.vel_idx, 0 : int(N_A / 2.0)] + ) + self.target_acc_on_line = self.a_bin_centers[self.acc_idx] + + target_vel = current_vel + self.target_acc_on_line / acc_kp_of_pure_pursuit + + # deceleration + if self.deceleration_rate <= achievement_rate: + if self.COURSE_NAME == "eight_course" or self.COURSE_NAME == "u_shaped_return": + target_vel = np.min([self.v_max / 10.0, max_vel_from_lateral_acc / 2.0]) + elif ( + self.COURSE_NAME == "straight_line_positive" + or self.COURSE_NAME == "straight_line_negative" + ): + target_vel = 0.0 + + # set target velocity on circle part + if part == "left_circle" or part == "right_circle": + if achievement_rate < 0.10 and self.target_vel_on_line > self.v_max / 2.0: + target_vel = np.min([self.v_max / 10.0, max_vel_from_lateral_acc / 2.0]) + elif achievement_rate < 0.50: + target_vel = max_vel_from_lateral_acc / 2.0 + else: + target_vel = max_vel_from_lateral_acc + + self.prev_part = part + + return target_vel + def updateNominalTargetTrajectory(self): data_collecting_area = np.array( [ @@ -371,8 +772,8 @@ def updateNominalTargetTrajectory(self): la = (l1 + l3) / 2 lb = (l2 + l4) / 2 if np.abs(la - lb) < 1e-6: - la += 0.1 # long_side_length must not be equal to short_side_length - ld = np.sqrt(la**2 + lb**2) + la += 0.1 # long_side_length must not be equal to short_side_length7 + rectangle_center_position = np.zeros(2) for i in range(4): rectangle_center_position[0] += data_collecting_area[i, 0] / 4.0 @@ -410,22 +811,52 @@ def updateNominalTargetTrajectory(self): long_side_margin = 5 long_side_margin = 5 - total_distance = ld * (1 + np.pi) * 2 actual_long_side = max(long_side_length - long_side_margin, 1.1) actual_short_side = max(short_side_length - long_side_margin, 1.0) - ( - trajectory_position_data, - trajectory_yaw_data, - trajectory_curvature_data, - self.trajectory_parts, - self.trajectory_achievement_rates, - ) = get_trajectory_points( - actual_long_side, - actual_short_side, - self.traj_step, - total_distance, - ) + + if self.COURSE_NAME == "eight_course": + ( + trajectory_position_data, + trajectory_yaw_data, + trajectory_curvature_data, + self.trajectory_parts, + self.trajectory_achievement_rates, + ) = get_eight_course_trajectory_points( + actual_long_side, actual_short_side, self.traj_step + ) + + elif ( + self.COURSE_NAME == "straight_line_positive" + or self.COURSE_NAME == "straight_line_negative" + ): + ( + trajectory_position_data, + trajectory_yaw_data, + trajectory_curvature_data, + self.trajectory_parts, + self.trajectory_achievement_rates, + ) = get_straight_line_course_trajectory_points( + actual_long_side, actual_short_side, self.traj_step, self.COURSE_NAME + ) + + elif self.COURSE_NAME == "u_shaped_return": + ( + trajectory_position_data, + trajectory_yaw_data, + trajectory_curvature_data, + self.trajectory_parts, + self.trajectory_achievement_rates, + ) = get_u_shaped_return_course_trajectory_points( + actual_long_side, actual_short_side, self.traj_step + ) + + else: + self.trajectory_position_data = None + self.trajectory_yaw_data = None + self.trajectory_longitudinal_velocity_data = None + self.trajectory_curvature_data = None + for i in range(len(trajectory_yaw_data)): if trajectory_yaw_data[i] > np.pi: trajectory_yaw_data[i] -= 2 * np.pi @@ -435,8 +866,8 @@ def updateNominalTargetTrajectory(self): # [2-2] translation and rotation of origin rot_matrix = np.array( [ - [cos(yaw_offset), -sin(yaw_offset)], - [sin(yaw_offset), cos(yaw_offset)], + [np.cos(yaw_offset), -np.sin(yaw_offset)], + [np.sin(yaw_offset), np.cos(yaw_offset)], ] ) trajectory_position_data = (rot_matrix @ trajectory_position_data.T).T @@ -502,13 +933,12 @@ def updateNominalTargetTrajectory(self): trajectory_curvature_data = ( 1 * np.convolve(augmented_curvature_data, w, mode="same")[window:-window] ) - # [2-4] nominal velocity target_longitudinal_velocity = ( self.get_parameter("target_longitudinal_velocity").get_parameter_value().double_value ) - trajectory_longitudinal_velocity_data = target_longitudinal_velocity * np.ones( + trajectory_longitudinal_velocity_data = target_longitudinal_velocity * np.zeros( len(trajectory_position_data) ) self.current_target_longitudinal_velocity = 1 * target_longitudinal_velocity @@ -525,118 +955,105 @@ def count_observations(self, v, a, steer): steer_bin = np.digitize(steer, self.steer_bins) - 1 a_bin = np.digitize(a, self.a_bins) - 1 - if 0 <= v_bin < self.num_bins and 0 <= a_bin < self.num_bins: + if 0 <= v_bin < self.num_bins_v and 0 <= a_bin < self.num_bins_a: self.collected_data_counts_of_vel_acc[v_bin, a_bin] += 1 - if 0 <= v_bin < self.num_bins and 0 <= steer_bin < self.num_bins: + if 0 <= v_bin < self.num_bins_v and 0 <= steer_bin < self.num_bins_steer: self.collected_data_counts_of_vel_steer[v_bin, steer_bin] += 1 - def plot_data_collection_grid(self, part, rate): - # do not update if enough time has not passed - if self.last_grid_update_time is not None: - time_elapsed = self.get_clock().now() - self.last_grid_update_time - if self.grid_update_time_interval > time_elapsed.nanoseconds / 1e9: - return + def plot_data_collection_grid(self): + self.axs[0].cla() + self.axs[0].scatter(self.acc_hist_for_plot, self.vel_hist_for_plot) + self.axs[0].plot(self.acc_hist_for_plot, self.vel_hist_for_plot) + self.axs[0].set_xlim([-2.0, 2.0]) + self.axs[0].set_ylim([0.0, self.v_max + 1.0]) + self.axs[0].set_xlabel("Acceleration") + self.axs[0].set_ylabel("Velocity") # update collected acceleration and velocity grid - for collection in self.axs[2].collections: + for collection in self.axs[1].collections: if collection.colorbar is not None: collection.colorbar.remove() - self.axs[2].cla() + self.axs[1].cla() self.heatmap = sns.heatmap( - self.collected_data_counts_of_vel_acc.T, + self.collected_data_counts_of_vel_acc_for_plot.T, annot=True, cmap="coolwarm", - xticklabels=np.round(self.v_bin_centers, 2), - yticklabels=np.round(self.a_bin_centers, 2), - ax=self.axs[2], + xticklabels=np.round(self.v_bin_centers_for_plot, 2), + yticklabels=np.round(self.a_bin_centers_for_plot, 2), + ax=self.axs[1], linewidths=0.1, linecolor="gray", ) - self.axs[2].set_xlabel("Velocity bins") - self.axs[2].set_ylabel("Acceleration bins") + self.axs[1].set_xlabel("Velocity bins") + self.axs[1].set_ylabel("Acceleration bins") - for collection in self.axs[3].collections: + for collection in self.axs[2].collections: if collection.colorbar is not None: collection.colorbar.remove() - self.axs[3].cla() + self.axs[2].cla() self.heatmap = sns.heatmap( - self.collected_data_counts_of_vel_steer.T, + self.collected_data_counts_of_vel_steer_for_plot.T, annot=True, cmap="coolwarm", - xticklabels=np.round(self.v_bin_centers, 2), - yticklabels=np.round(self.steer_bin_centers, 2), - ax=self.axs[3], + xticklabels=np.round(self.v_bin_centers_for_plot, 2), + yticklabels=np.round(self.steer_bin_centers_for_plot, 2), + ax=self.axs[2], linewidths=0.1, linecolor="gray", ) # update collected steer and velocity grid - self.axs[3].set_xlabel("Velocity bins") - self.axs[3].set_ylabel("Steer bins") + self.axs[2].set_xlabel("Velocity bins") + self.axs[2].set_ylabel("Steer bins") - self.axs[1].cla() - self.axs[1].scatter(self.acc_hist, self.vel_hist) - self.axs[1].plot(self.acc_hist, self.vel_hist) - self.axs[1].set_xlim([-2.0, 2.0]) - self.axs[1].set_ylim([0.0, self.v_max + 1.0]) - self.axs[1].set_xlabel("Acceleration") - self.axs[1].set_ylabel("Velocity") - self.last_grid_update_time = self.get_clock().now() + self.fig.canvas.draw() - data_collecting_area = np.array( - [ - np.array( - [ - self._data_collecting_area_polygon.polygon.points[i].x, - self._data_collecting_area_polygon.polygon.points[i].y, - self._data_collecting_area_polygon.polygon.points[i].z, - ] - ) - for i in range(4) - ] - ) + plt.pause(0.005) - l1 = np.sqrt(((data_collecting_area[0, :2] - data_collecting_area[1, :2]) ** 2).sum()) - l2 = np.sqrt(((data_collecting_area[1, :2] - data_collecting_area[2, :2]) ** 2).sum()) - l3 = np.sqrt(((data_collecting_area[2, :2] - data_collecting_area[3, :2]) ** 2).sum()) - l4 = np.sqrt(((data_collecting_area[3, :2] - data_collecting_area[0, :2]) ** 2).sum()) - la = (l1 + l3) / 2 - lb = (l2 + l4) / 2 - title_of_fig = "rectangle : (la,lb) = " + str((la, lb)) - self.fig.suptitle(title_of_fig) + def timer_callback_plot(self): + with self.lock: + self.collected_data_counts_of_vel_acc_for_plot = ( + self.collected_data_counts_of_vel_acc.copy() + ) + self.collected_data_counts_of_vel_steer_for_plot = ( + self.collected_data_counts_of_vel_steer.copy() + ) - self.fig.canvas.draw() + self.acc_hist_for_plot = self.acc_hist.copy() + self.vel_hist_for_plot = self.vel_hist.copy() + + self.v_bin_centers_for_plot = self.v_bin_centers + self.steer_bin_centers_for_plot = self.steer_bin_centers + self.a_bin_centers_for_plot = self.a_bin_centers - def timer_callback(self): + self.plot_data_collection_grid() + plt.pause(0.01) + + def timer_callback_traj(self): if ( self._present_kinematic_state is not None and self._present_acceleration is not None and self.trajectory_position_data is not None ): - yaw = getYaw( - np.array( - [ - self._present_kinematic_state.pose.pose.orientation.x, - self._present_kinematic_state.pose.pose.orientation.y, - self._present_kinematic_state.pose.pose.orientation.z, - self._present_kinematic_state.pose.pose.orientation.w, - ] - ) - ) + # calculate steer + angular_z = self._present_kinematic_state.twist.twist.angular.z wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value - steer = np.arctan2(wheel_base * yaw, self._present_kinematic_state.twist.twist.linear.x) + steer = arctan2( + wheel_base * angular_z, self._present_kinematic_state.twist.twist.linear.x + ) # update velocity and acceleration bin if ego vehicle is moving if self._present_kinematic_state.twist.twist.linear.x > 1e-3: - self.count_observations( - self._present_kinematic_state.twist.twist.linear.x, - self._present_acceleration.accel.accel.linear.x, - steer, - ) + with self.lock: + self.count_observations( + self._present_kinematic_state.twist.twist.linear.x, + self._present_acceleration.accel.accel.linear.x, + steer, + ) # [0] update nominal target trajectory if changing related ros2 params target_longitudinal_velocity = ( @@ -717,7 +1134,7 @@ def timer_callback(self): ) # 4 is minimum noise_data_num for i in range(noise_data_num): self.vel_noise_list.append( - tmp_noise_vel * sin(2.0 * np.pi * i / noise_data_num) + tmp_noise_vel * np.sin(2.0 * np.pi * i / noise_data_num) ) self.vel_noise_list.pop(0) @@ -748,161 +1165,12 @@ def timer_callback(self): self.one_round_progress_rate = 1.0 * nearestIndex / len(trajectory_position_data) # set target velocity - part = self.trajectory_parts[ - nearestIndex - ] # "left_circle", "right_circle", "linear_positive", "linear_negative" - achievement_rate = self.trajectory_achievement_rates[nearestIndex] - current_vel = self._present_kinematic_state.twist.twist.linear.x - current_acc = self._present_acceleration.accel.accel.linear.x - - acc_kp_of_pure_pursuit = self.get_parameter("acc_kp").get_parameter_value().double_value - N = self.num_bins - - self.acc_hist[:-1] = 1.0 * self.acc_hist[1:] - self.acc_hist[-1] = current_acc - self.vel_hist[:-1] = 1.0 * self.vel_hist[1:] - self.vel_hist[-1] = current_vel - - max_lateral_accel = ( - self.get_parameter("max_lateral_accel").get_parameter_value().double_value - ) - if USE_CURVATURE_RADIUS_FLAG: - max_vel_from_lateral_acc = np.sqrt( - max_lateral_accel * trajectory_curvature_data[nearestIndex] - ) - else: - max_vel_from_lateral_acc = np.sqrt( - max_lateral_accel / trajectory_curvature_data[nearestIndex] - ) - - target_vel = min([self.v_max / N, max_vel_from_lateral_acc]) - - # set self.target_acc_on_line and self.target_vel_on_line after vehicle from circle part to linear part - min_data_num_margin = 10 - min_index_list = [] - if (self.prev_part == "left_circle" or self.prev_part == "right_circle") and ( - part == "linear_positive" or part == "linear_negative" - ): - self.on_line_vel_flag = True - min_num_data = 1e12 - - # do not collect data when velocity and acceleration are low - exclude_idx_list = [(0, 0), (1, 0), (2, 0), (0, 1), (1, 1), (0, 2)] - # do not collect data when velocity and acceleration are high - exclude_idx_list += [ - (-1 + N, -1 + N), - (-2 + N, -1 + N), - (-3 + N, -1 + N), - (-1 + N, -2 + N), - (-2 + N, -2 + N), - (-1 + N, -3 + N), - ] - - for i in range(0, N): - for j in range(0, N): - if (i, j) not in exclude_idx_list: - if ( - min_num_data - min_data_num_margin - > self.collected_data_counts_of_vel_acc[i, j] - ): - min_num_data = self.collected_data_counts_of_vel_acc[i, j] - min_index_list.clear() - min_index_list.append((j, i)) - - elif ( - min_num_data + min_data_num_margin - > self.collected_data_counts_of_vel_acc[i, j] - ): - min_index_list.append((j, i)) - - self.acc_idx, self.vel_idx = min_index_list[ - np.random.randint(0, len(min_index_list)) - ] - self.target_acc_on_line = self.a_bin_centers[self.acc_idx] - self.target_vel_on_line = self.v_bin_centers[self.vel_idx] - - if self.target_vel_on_line > self.v_max * 3.0 / 4.0: - self.deceleration_rate = 0.55 - elif self.target_vel_on_line > self.v_max / 2.0: - self.deceleration_rate = 0.60 - else: - self.deceleration_rate = 1.0 - - # set target velocity on linear part - if part == "linear_positive" or part == "linear_negative": - if ( - current_vel > self.target_vel_on_line - self.v_max / N / 8.0 - and self.target_vel_on_line >= self.v_max / 2.0 - ): - self.on_line_vel_flag = False - elif ( - abs(current_vel - self.target_vel_on_line) < self.v_max / N / 4.0 - and self.target_vel_on_line < self.v_max / 2.0 - ): - self.on_line_vel_flag = False - - # accelerate until vehicle reaches target_vel_on_line - if 0.0 <= achievement_rate and achievement_rate < 0.45 and self.on_line_vel_flag: - target_vel = self.target_vel_on_line - - if ( - current_vel > self.target_vel_on_line - self.v_max / N - and self.target_acc_on_line > 2.0 * self.a_max / N - ): - target_vel = current_vel + self.target_acc_on_line / acc_kp_of_pure_pursuit - - # collect target_acceleration data when current velocity is close to target_vel_on_line - elif ( - achievement_rate < self.deceleration_rate - or self.target_vel_on_line < self.v_max / 2.0 - ): - if self.collected_data_counts_of_vel_acc[self.vel_idx, self.acc_idx] > 50: - self.acc_idx = np.argmin( - self.collected_data_counts_of_vel_acc[self.vel_idx, :] - ) - self.target_acc_on_line = self.a_bin_centers[self.acc_idx] - - if ( - current_vel - < max([self.target_vel_on_line - 1.0 * self.v_max / N, self.v_max / N]) - and self.target_acc_on_line < 0.0 - ): - self.acc_idx = np.argmin( - self.collected_data_counts_of_vel_acc[self.vel_idx, int(N / 2.0) : N] - ) + int(N / 2) - self.target_acc_on_line = self.a_bin_centers[self.acc_idx] - - elif ( - current_vel > self.target_vel_on_line + 1.0 * self.v_max / N - and self.target_acc_on_line >= 0.0 - ): - self.acc_idx = np.argmin( - self.collected_data_counts_of_vel_acc[self.vel_idx, 0 : int(N / 2.0)] - ) - self.target_acc_on_line = self.a_bin_centers[self.acc_idx] - - target_vel = current_vel + self.target_acc_on_line / acc_kp_of_pure_pursuit - - # deceleration - if ( - self.deceleration_rate <= achievement_rate - and self.target_vel_on_line > self.v_max / 2.0 - ): - target_vel = 2.0 * self.v_max / N - - # set target velocity on circle part - if part == "left_circle" or part == "right_circle": - if achievement_rate < 0.10 and self.target_vel_on_line > self.v_max / 2.0: - target_vel = self.v_max / N - elif achievement_rate < 0.50: - target_vel = max_vel_from_lateral_acc / 2.0 - else: - target_vel = max_vel_from_lateral_acc + with self.lock: + target_vel = self.get_target_velocity(nearestIndex) trajectory_longitudinal_velocity_data = np.array( [target_vel for _ in range(len(trajectory_longitudinal_velocity_data))] ) - self.prev_part = part # [5] modify target velocity # [5-1] add noise @@ -927,6 +1195,9 @@ def timer_callback(self): ) # [5-2] apply lateral accel limit + max_lateral_accel = ( + self.get_parameter("max_lateral_accel").get_parameter_value().double_value + ) if USE_CURVATURE_RADIUS_FLAG: lateral_acc_limit = np.sqrt(max_lateral_accel * trajectory_curvature_data) else: @@ -996,8 +1267,12 @@ def timer_callback(self): tmp_traj_point.pose.orientation.x = 0.0 tmp_traj_point.pose.orientation.y = 0.0 - tmp_traj_point.pose.orientation.z = sin(trajectory_yaw_data[i + nearestIndex] / 2) - tmp_traj_point.pose.orientation.w = cos(trajectory_yaw_data[i + nearestIndex] / 2) + tmp_traj_point.pose.orientation.z = np.sin( + trajectory_yaw_data[i + nearestIndex] / 2 + ) + tmp_traj_point.pose.orientation.w = np.cos( + trajectory_yaw_data[i + nearestIndex] / 2 + ) tmp_traj_point.longitudinal_velocity_mps = trajectory_longitudinal_velocity_data[ i + nearestIndex @@ -1073,7 +1348,7 @@ def timer_callback(self): self.data_collecting_trajectory_marker_array_pub_.publish(marker_array) if debug_matplotlib_plot_flag: - self.axs[0].cla() + self.axs_debug.cla() step_size_array = np.sqrt( ((trajectory_position_data[1:] - trajectory_position_data[:-1]) ** 2).sum( axis=1 @@ -1091,9 +1366,9 @@ def timer_callback(self): timestamp[i] = timestamp[i - 1] + time_width_array[i - 1] timestamp -= timestamp[nearestIndex] - self.axs[0].plot(0, present_linear_velocity[0], "o", label="current vel") + self.axs_debug.plot(0, present_linear_velocity[0], "o", label="current vel") - self.axs[0].plot( + self.axs_debug.plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], trajectory_longitudinal_velocity_data_without_limit[ nearestIndex : nearestIndex + pub_traj_len @@ -1101,32 +1376,31 @@ def timer_callback(self): "--", label="target vel before applying limit", ) - self.axs[0].plot( + self.axs_debug.plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], lateral_acc_limit[nearestIndex : nearestIndex + pub_traj_len], "--", label="lateral acc limit (always)", ) - self.axs[0].plot( + self.axs_debug.plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], velocity_limit_by_tracking_error * np.ones(pub_traj_len), "--", label="vel limit by tracking error (only when exceeding threshold)", ) - self.axs[0].plot( + self.axs_debug.plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], trajectory_longitudinal_velocity_data[ nearestIndex : nearestIndex + pub_traj_len ], label="actual target vel", ) - self.axs[0].set_xlim([-0.5, 10.5]) - self.axs[0].set_ylim([-0.5, 12.5]) + self.axs_debug.set_xlim([-0.5, 10.5]) + self.axs_debug.set_ylim([-0.5, 12.5]) - self.axs[0].set_xlabel("future timestamp [s]") - self.axs[0].set_ylabel("longitudinal_velocity [m/s]") - self.axs[0].legend(fontsize=8) - self.plot_data_collection_grid(part, achievement_rate) + self.axs_debug.set_xlabel("future timestamp [s]") + self.axs_debug.set_ylabel("longitudinal_velocity [m/s]") + self.axs_debug.legend(fontsize=8) plt.pause(0.01)