From 1cdbd05c8a73620e0b7aeaed2fe683e6167d0ab5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 2 Dec 2024 15:41:08 +0000 Subject: [PATCH] style(pre-commit): autofix --- control_data_collecting_tool/README.md | 2 - .../scripts/courses/along_road.py | 11 +- .../scripts/courses/base_course.py | 8 +- .../scripts/courses/figure_eight.py | 5 +- .../scripts/courses/load_course.py | 2 + .../scripts/courses/reversal_loop_circle.py | 11 +- .../scripts/courses/straight_line_negative.py | 5 +- .../scripts/courses/straight_line_positive.py | 5 +- .../scripts/courses/u_shaped.py | 5 +- .../scripts/data_collecting_base_node.py | 47 +++-- .../scripts/data_collecting_data_counter.py | 5 +- .../scripts/data_collecting_plotter.py | 65 +++---- .../data_collecting_trajectory_publisher.py | 9 +- .../scripts/mask/mask_selector.py | 163 +++++++++++++----- 14 files changed, 214 insertions(+), 129 deletions(-) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 9a2b6301..0a3c1427 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -82,10 +82,8 @@ This package provides tools for automatically collecting data using pure pursuit - `/data_collecting_lookahead_marker_array` - Type: MarkerArray - 6. The following actions differ depending on the selected course. If you select the trajectory from [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`, `reversal_loop_circle`], proceed to 6.1. If you select the trajectory from [`along_road`], please proceed to 6.2. - - 6.1 If you choose the trajectory from [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`, `reversal_loop_circle`], select `DataCollectingAreaSelectionTool` plugin. diff --git a/control_data_collecting_tool/scripts/courses/along_road.py b/control_data_collecting_tool/scripts/courses/along_road.py index 7589a726..e1293a67 100644 --- a/control_data_collecting_tool/scripts/courses/along_road.py +++ b/control_data_collecting_tool/scripts/courses/along_road.py @@ -14,12 +14,13 @@ # See the License for the specific language governing permissions and # limitations under the License. +from autoware_map_msgs.msg import LaneletMapBin from courses.base_course import Base_Course from courses.lanelet import LaneletMapHandler import numpy as np from rcl_interfaces.msg import ParameterDescriptor -from rclpy.qos import QoSProfile, QoSDurabilityPolicy -from autoware_map_msgs.msg import LaneletMapBin +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile from scipy.interpolate import interp1d as interpolation_1d @@ -95,6 +96,7 @@ def declare_along_road_params(node): ), ) + class Along_Road(Base_Course): def __init__(self, step: float, param_dict): super().__init__(step, param_dict) @@ -252,7 +254,7 @@ def get_target_velocity( collected_data_counts_of_vel_acc, collected_data_counts_of_vel_steer, mask_vel_acc, - mask_vel_steer + mask_vel_steer, ): part = self.parts[nearestIndex] achievement_rate = self.achievement_rates[nearestIndex] @@ -264,8 +266,7 @@ def get_target_velocity( or (part == "straight" and achievement_rate < 0.05) ) and not self.set_target_velocity_on_straight_line: self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( - collected_data_counts_of_vel_acc, - mask_vel_acc + collected_data_counts_of_vel_acc, mask_vel_acc ) self.target_acc_on_straight_line = self.params.a_bin_centers[self.acc_idx] self.target_vel_on_straight_line = self.params.v_bin_centers[self.vel_idx] diff --git a/control_data_collecting_tool/scripts/courses/base_course.py b/control_data_collecting_tool/scripts/courses/base_course.py index 01fb2658..5d4ccf92 100644 --- a/control_data_collecting_tool/scripts/courses/base_course.py +++ b/control_data_collecting_tool/scripts/courses/base_course.py @@ -57,13 +57,15 @@ def choose_target_velocity_acc(self, collected_data_counts_of_vel_acc, mask_vel_ for j in range( self.params.collecting_data_min_n_a, self.params.collecting_data_max_n_a ): - if mask_vel_acc[i,j] == 1: + if mask_vel_acc[i, j] == 1: if min_num_data - min_data_num_margin > collected_data_counts_of_vel_acc[i, j]: min_num_data = 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 > collected_data_counts_of_vel_acc[i, j]: + elif ( + min_num_data + min_data_num_margin > collected_data_counts_of_vel_acc[i, j] + ): min_index_list.append((j, i)) return min_index_list[np.random.randint(0, len(min_index_list))] @@ -77,7 +79,7 @@ def get_target_velocity( collected_data_counts_of_vel_acc, collected_data_counts_of_vel_steer, mask_vel_acc, - mask_vel_steer + mask_vel_steer, ): pass diff --git a/control_data_collecting_tool/scripts/courses/figure_eight.py b/control_data_collecting_tool/scripts/courses/figure_eight.py index 4e75fa3e..71e4e2fe 100644 --- a/control_data_collecting_tool/scripts/courses/figure_eight.py +++ b/control_data_collecting_tool/scripts/courses/figure_eight.py @@ -178,7 +178,7 @@ def get_target_velocity( collected_data_counts_of_vel_acc, collected_data_counts_of_vel_steer, mask_vel_acc, - mask_vel_steer + mask_vel_steer, ): part = self.parts[nearestIndex] achievement_rate = self.achievement_rates[nearestIndex] @@ -190,8 +190,7 @@ def get_target_velocity( or (part == "straight" and achievement_rate < 0.05) ) and not self.set_target_velocity_on_straight_line: self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( - collected_data_counts_of_vel_acc, - mask_vel_acc + collected_data_counts_of_vel_acc, mask_vel_acc ) self.target_acc_on_straight_line = self.params.a_bin_centers[self.acc_idx] self.target_vel_on_straight_line = self.params.v_bin_centers[self.vel_idx] diff --git a/control_data_collecting_tool/scripts/courses/load_course.py b/control_data_collecting_tool/scripts/courses/load_course.py index a3314093..4e1dea9f 100644 --- a/control_data_collecting_tool/scripts/courses/load_course.py +++ b/control_data_collecting_tool/scripts/courses/load_course.py @@ -44,6 +44,7 @@ def declare_course_params(course_name, node): elif course_name == "along_road": declare_along_road_params(node) + def create_course_subscription(course_name, node): if course_name == "eight_course": pass @@ -58,6 +59,7 @@ def create_course_subscription(course_name, node): elif course_name == "along_road": pass + def load_course(course_name, step_size, params_dict): # Load the course based on the course name if course_name == "eight_course": 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 a8e059d7..503c3073 100644 --- a/control_data_collecting_tool/scripts/courses/reversal_loop_circle.py +++ b/control_data_collecting_tool/scripts/courses/reversal_loop_circle.py @@ -1012,7 +1012,7 @@ def get_target_velocity( collected_data_counts_of_vel_acc, collected_data_counts_of_vel_steer, mask_vel_acc, - mask_vel_steer + mask_vel_steer, ): # Calculate the target velocity for the vehicle based on its current state, trajectory phase, and constraints such as acceleration limits and trajectory curvature. @@ -1020,8 +1020,7 @@ def get_target_velocity( if not self.updated_target_velocity: # Choose velocity and acceleration bins based on collected data self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( - collected_data_counts_of_vel_acc, - mask_vel_acc + collected_data_counts_of_vel_acc, mask_vel_acc ) self.target_acc_on_segmentation = self.params.a_bin_centers[self.acc_idx] self.target_vel_on_segmentation = self.params.v_bin_centers[self.vel_idx] @@ -1131,7 +1130,11 @@ def get_target_velocity( ) # Determine the minimum velocity based on collected data - min_vel = np.min((collected_data_counts_of_vel_steer + 1e9 * (1-mask_vel_steer) )[:max_vel_idx, steer_idx]) + min_vel = np.min( + (collected_data_counts_of_vel_steer + 1e9 * (1 - mask_vel_steer))[ + :max_vel_idx, steer_idx + ] + ) vel_idx = np.where( collected_data_counts_of_vel_steer[:max_vel_idx, steer_idx] == min_vel )[0] diff --git a/control_data_collecting_tool/scripts/courses/straight_line_negative.py b/control_data_collecting_tool/scripts/courses/straight_line_negative.py index 5915f6d8..74270236 100644 --- a/control_data_collecting_tool/scripts/courses/straight_line_negative.py +++ b/control_data_collecting_tool/scripts/courses/straight_line_negative.py @@ -76,7 +76,7 @@ def get_target_velocity( collected_data_counts_of_vel_acc, collected_data_counts_of_vel_steer, mask_vel_acc, - mask_vel_steer + mask_vel_steer, ): part = self.parts[nearestIndex] achievement_rate = self.achievement_rates[nearestIndex] @@ -85,8 +85,7 @@ def get_target_velocity( # Check and update target velocity on straight line if part == "straight" and achievement_rate < 0.05: self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( - collected_data_counts_of_vel_acc, - mask_vel_acc + collected_data_counts_of_vel_acc, mask_vel_acc ) self.target_acc_on_straight_line = self.params.a_bin_centers[self.acc_idx] self.target_vel_on_straight_line = self.params.v_bin_centers[self.vel_idx] diff --git a/control_data_collecting_tool/scripts/courses/straight_line_positive.py b/control_data_collecting_tool/scripts/courses/straight_line_positive.py index 54c23550..c07fda1a 100644 --- a/control_data_collecting_tool/scripts/courses/straight_line_positive.py +++ b/control_data_collecting_tool/scripts/courses/straight_line_positive.py @@ -76,7 +76,7 @@ def get_target_velocity( collected_data_counts_of_vel_acc, collected_data_counts_of_vel_steer, mask_vel_acc, - mask_vel_steer + mask_vel_steer, ): part = self.parts[nearestIndex] achievement_rate = self.achievement_rates[nearestIndex] @@ -85,8 +85,7 @@ def get_target_velocity( # Check and update target velocity on straight line if part == "straight" and achievement_rate < 0.05: self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( - collected_data_counts_of_vel_acc, - mask_vel_acc + collected_data_counts_of_vel_acc, mask_vel_acc ) self.target_acc_on_straight_line = self.params.a_bin_centers[self.acc_idx] self.target_vel_on_straight_line = self.params.v_bin_centers[self.vel_idx] diff --git a/control_data_collecting_tool/scripts/courses/u_shaped.py b/control_data_collecting_tool/scripts/courses/u_shaped.py index dd36d99e..ac33b1e7 100644 --- a/control_data_collecting_tool/scripts/courses/u_shaped.py +++ b/control_data_collecting_tool/scripts/courses/u_shaped.py @@ -146,7 +146,7 @@ def get_target_velocity( collected_data_counts_of_vel_acc, collected_data_counts_of_vel_steer, mask_vel_acc, - mask_vel_steer + mask_vel_steer, ): part = self.parts[nearestIndex] achievement_rate = self.achievement_rates[nearestIndex] @@ -158,8 +158,7 @@ def get_target_velocity( or (part == "straight" and achievement_rate < 0.05) ) and not self.set_target_velocity_on_straight_line: self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( - collected_data_counts_of_vel_acc, - mask_vel_acc + collected_data_counts_of_vel_acc, mask_vel_acc ) self.target_acc_on_straight_line = self.params.a_bin_centers[self.acc_idx] self.target_vel_on_straight_line = self.params.v_bin_centers[self.vel_idx] diff --git a/control_data_collecting_tool/scripts/data_collecting_base_node.py b/control_data_collecting_tool/scripts/data_collecting_base_node.py index d81da1ff..b42cb092 100644 --- a/control_data_collecting_tool/scripts/data_collecting_base_node.py +++ b/control_data_collecting_tool/scripts/data_collecting_base_node.py @@ -14,15 +14,16 @@ # 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 from autoware_adapi_v1_msgs.msg import OperationModeState from autoware_vehicle_msgs.msg import ControlModeReport from geometry_msgs.msg import AccelWithCovarianceStamped from nav_msgs.msg import Odometry -import os import numpy as np from rcl_interfaces.msg import ParameterDescriptor from rclpy.node import Node -from ament_index_python.packages import get_package_share_directory class DataCollectingBaseNode(Node): @@ -43,9 +44,7 @@ def __init__(self, node_name): self.declare_parameter( "MASK_NAME", "default", - ParameterDescriptor( - description="Masks for Data collection" - ), + ParameterDescriptor(description="Masks for Data collection"), ) self.declare_parameter( @@ -205,7 +204,9 @@ def __init__(self, node_name): 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.steer_rate_bins = np.linspace(self.steer_rate_min, self.steer_rate_max, self.num_bins_steer_rate + 1) + self.steer_rate_bins = np.linspace( + self.steer_rate_min, self.steer_rate_max, self.num_bins_steer_rate + 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 @@ -217,16 +218,32 @@ def __init__(self, node_name): """ # set mask name MASK_NAME = self.get_parameter("MASK_NAME").value - mask_directory_path = get_package_share_directory("control_data_collecting_tool") + "/config/masks/" + MASK_NAME + mask_directory_path = ( + get_package_share_directory("control_data_collecting_tool") + + "/config/masks/" + + MASK_NAME + ) - mask_vel_acc_path = os.path.join(mask_directory_path, f"{MASK_NAME}_Velocity_Acceleration.txt") - self.mask_vel_acc = self.load_mask_from_txt(mask_vel_acc_path, self.num_bins_v, self.num_bins_a) + mask_vel_acc_path = os.path.join( + mask_directory_path, f"{MASK_NAME}_Velocity_Acceleration.txt" + ) + self.mask_vel_acc = self.load_mask_from_txt( + mask_vel_acc_path, self.num_bins_v, self.num_bins_a + ) - mask_velocity_steering_path = os.path.join(mask_directory_path, f"{MASK_NAME}_Velocity_Steering.txt") - self.mask_vel_steer = self.load_mask_from_txt(mask_velocity_steering_path, self.num_bins_v, self.num_bins_steer) + mask_velocity_steering_path = os.path.join( + mask_directory_path, f"{MASK_NAME}_Velocity_Steering.txt" + ) + self.mask_vel_steer = self.load_mask_from_txt( + mask_velocity_steering_path, self.num_bins_v, self.num_bins_steer + ) - mask_velocity_steer_rate_path = os.path.join(mask_directory_path, f"{MASK_NAME}_Velocity_Steering_Rate.txt") - self.mask_vel_steer_rate = self.load_mask_from_txt(mask_velocity_steer_rate_path, self.num_bins_v, self.num_bins_steer_rate) + mask_velocity_steer_rate_path = os.path.join( + mask_directory_path, f"{MASK_NAME}_Velocity_Steering_Rate.txt" + ) + self.mask_vel_steer_rate = self.load_mask_from_txt( + mask_velocity_steer_rate_path, self.num_bins_v, self.num_bins_steer_rate + ) def onOdometry(self, msg): self._present_kinematic_state = msg @@ -245,7 +262,7 @@ def subscribe_operation_mode(self, msg): def subscribe_control_mode(self, msg): self._present_control_mode_ = msg.mode - + def load_mask_from_txt(self, file_path, nx, ny): """ Loads a numerical mask from a text file into a numpy array. @@ -262,5 +279,5 @@ def load_mask_from_txt(self, file_path, nx, ny): return mask except Exception as e: pass - + return np.ones((nx, ny), dtype=int) diff --git a/control_data_collecting_tool/scripts/data_collecting_data_counter.py b/control_data_collecting_tool/scripts/data_collecting_data_counter.py index fff74914..ba49b2f9 100755 --- a/control_data_collecting_tool/scripts/data_collecting_data_counter.py +++ b/control_data_collecting_tool/scripts/data_collecting_data_counter.py @@ -77,7 +77,9 @@ def __init__(self): Int32MultiArray, "/control_data_collecting_tools/collected_data_counts_of_vel_steer", 10 ) self.collected_data_counts_of_vel_steer_rate_publisher_ = self.create_publisher( - Int32MultiArray, "/control_data_collecting_tools/collected_data_counts_of_vel_steer_rate", 10 + Int32MultiArray, + "/control_data_collecting_tools/collected_data_counts_of_vel_steer_rate", + 10, ) self.vel_hist_publisher_ = self.create_publisher( @@ -225,7 +227,6 @@ def count_observations(self, v, a, steer, steer_rate): if 0 <= v_bin < self.num_bins_v and 0 <= steer_rate_bin < self.num_bins_steer_rate: self.collected_data_counts_of_vel_steer_rate[v_bin, steer_rate_bin] += 1 - # call back for counting data points def timer_callback_counter(self): diff --git a/control_data_collecting_tool/scripts/data_collecting_plotter.py b/control_data_collecting_tool/scripts/data_collecting_plotter.py index dfb330e4..54905efb 100755 --- a/control_data_collecting_tool/scripts/data_collecting_plotter.py +++ b/control_data_collecting_tool/scripts/data_collecting_plotter.py @@ -15,16 +15,16 @@ # limitations under the License. from data_collecting_base_node import DataCollectingBaseNode -import matplotlib.pyplot as plt -from matplotlib.patches import Rectangle from matplotlib.colors import ListedColormap from matplotlib.colors import Normalize +from matplotlib.patches import Rectangle +import matplotlib.pyplot as plt import numpy as np +from rcl_interfaces.msg import ParameterDescriptor import rclpy import seaborn as sns from std_msgs.msg import Float32MultiArray from std_msgs.msg import Int32MultiArray -from rcl_interfaces.msg import ParameterDescriptor class DataCollectingPlotter(DataCollectingBaseNode): @@ -55,9 +55,15 @@ def __init__(self): ), ) - self.VEL_ACC_THRESHOLD = self.get_parameter("VEL_ACC_THRESHOLD").get_parameter_value().integer_value - self.VEL_STEER_THRESHOLD = self.get_parameter("VEL_STEER_THRESHOLD").get_parameter_value().integer_value - self.VEL_STEER_RATE_THRESHOLD = self.get_parameter("VEL_STEER_RATE_THRESHOLD").get_parameter_value().integer_value + self.VEL_ACC_THRESHOLD = ( + self.get_parameter("VEL_ACC_THRESHOLD").get_parameter_value().integer_value + ) + self.VEL_STEER_THRESHOLD = ( + self.get_parameter("VEL_STEER_THRESHOLD").get_parameter_value().integer_value + ) + self.VEL_STEER_RATE_THRESHOLD = ( + self.get_parameter("VEL_STEER_RATE_THRESHOLD").get_parameter_value().integer_value + ) # callback for plot self.grid_update_time_interval = 5.0 @@ -69,9 +75,9 @@ def __init__(self): self.fig, self.axs = plt.subplots(4, 1, figsize=(12, 20)) plt.ion() self.cmap = ListedColormap(["blue", "yellowgreen"]) - self.vel_acc_heatmap_norm = Normalize(vmin=0, vmax=2*self.VEL_ACC_THRESHOLD) - self.vel_steer_heatmap_norm = Normalize(vmin=0, vmax=2*self.VEL_STEER_THRESHOLD) - self.vel_steer_rate_heatmap_norm = Normalize(vmin=0, vmax=2*self.VEL_STEER_RATE_THRESHOLD) + self.vel_acc_heatmap_norm = Normalize(vmin=0, vmax=2 * self.VEL_ACC_THRESHOLD) + self.vel_steer_heatmap_norm = Normalize(vmin=0, vmax=2 * self.VEL_STEER_THRESHOLD) + self.vel_steer_rate_heatmap_norm = Normalize(vmin=0, vmax=2 * self.VEL_STEER_RATE_THRESHOLD) self.collected_data_counts_of_vel_acc_subscription_ = self.create_subscription( Int32MultiArray, @@ -142,7 +148,6 @@ def timer_callback_plotter(self): plt.pause(0.1) def plot_data_collection_grid(self): - self.axs[0].cla() self.axs[0].scatter(self.acc_hist, self.vel_hist) self.axs[0].plot(self.acc_hist, self.vel_hist) @@ -167,7 +172,7 @@ def plot_data_collection_grid(self): ax=self.axs[1], linewidths=0.1, linecolor="gray", - cbar_kws={'ticks':[0, self.VEL_ACC_THRESHOLD]} + cbar_kws={"ticks": [0, self.VEL_ACC_THRESHOLD]}, ) self.axs[1].set_xlabel("Velocity bins") @@ -178,22 +183,15 @@ def plot_data_collection_grid(self): for j in range(self.collected_data_counts_of_vel_acc.shape[1]): if self.mask_vel_acc[i, j] == 1: rect = Rectangle( - (i, j), 1, 1, - linewidth=0.25, - edgecolor='black', - facecolor='none' + (i, j), 1, 1, linewidth=0.25, edgecolor="black", facecolor="none" ) - #self.axs[1].add_patch(rect) + # self.axs[1].add_patch(rect) else: rect = Rectangle( - (i, j), 1, 1, - linewidth=0.25, - edgecolor='black', - facecolor='gray' + (i, j), 1, 1, linewidth=0.25, edgecolor="black", facecolor="gray" ) self.axs[1].add_patch(rect) - for collection in self.axs[2].collections: if collection.colorbar is not None: collection.colorbar.remove() @@ -209,7 +207,7 @@ def plot_data_collection_grid(self): ax=self.axs[2], linewidths=0.1, linecolor="gray", - cbar_kws={'ticks':[0, self.VEL_STEER_THRESHOLD]} + cbar_kws={"ticks": [0, self.VEL_STEER_THRESHOLD]}, ) # Display mask @@ -217,18 +215,12 @@ def plot_data_collection_grid(self): for j in range(self.collected_data_counts_of_vel_steer.shape[1]): if self.mask_vel_steer[i, j] == 1: rect = Rectangle( - (i, j), 1, 1, - linewidth=0.25, - edgecolor='black', - facecolor='none' + (i, j), 1, 1, linewidth=0.25, edgecolor="black", facecolor="none" ) self.axs[2].add_patch(rect) else: rect = Rectangle( - (i, j), 1, 1, - linewidth=0.25, - edgecolor='black', - facecolor='gray' + (i, j), 1, 1, linewidth=0.25, edgecolor="black", facecolor="gray" ) self.axs[2].add_patch(rect) @@ -236,7 +228,6 @@ def plot_data_collection_grid(self): self.axs[2].set_xlabel("Velocity bins") self.axs[2].set_ylabel("Steer bins") - for collection in self.axs[3].collections: if collection.colorbar is not None: collection.colorbar.remove() @@ -252,7 +243,7 @@ def plot_data_collection_grid(self): ax=self.axs[3], linewidths=0.1, linecolor="gray", - cbar_kws={'ticks':[0, self.VEL_STEER_THRESHOLD]} + cbar_kws={"ticks": [0, self.VEL_STEER_THRESHOLD]}, ) # Display mask @@ -260,18 +251,12 @@ def plot_data_collection_grid(self): for j in range(self.collected_data_counts_of_vel_steer_rate.shape[1]): if self.mask_vel_steer_rate[i, j] == 1: rect = Rectangle( - (i, j), 1, 1, - linewidth=0.25, - edgecolor='black', - facecolor='none' + (i, j), 1, 1, linewidth=0.25, edgecolor="black", facecolor="none" ) self.axs[3].add_patch(rect) else: rect = Rectangle( - (i, j), 1, 1, - linewidth=0.25, - edgecolor='black', - facecolor='gray' + (i, j), 1, 1, linewidth=0.25, edgecolor="black", facecolor="gray" ) self.axs[3].add_patch(rect) 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 dc6c775e..54a9bfea 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -14,10 +14,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +import queue + from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint -from courses.load_course import declare_course_params from courses.load_course import create_course_subscription +from courses.load_course import declare_course_params from courses.load_course import load_course from data_collecting_base_node import DataCollectingBaseNode from geometry_msgs.msg import Point @@ -26,7 +28,6 @@ import matplotlib.pyplot as plt import numpy as np from numpy import pi -import queue from rcl_interfaces.msg import ParameterDescriptor import rclpy from scipy.spatial.transform import Rotation as R @@ -203,7 +204,7 @@ def __init__(self): self.updated_course_param_queue = queue.Queue() create_course_subscription(self.COURSE_NAME, self) - + # obtain ros params as dictionary param_names = self._parameters params = self.get_parameters(param_names) @@ -533,7 +534,7 @@ def callback_trajectory_generator(self): self.collected_data_counts_of_vel_acc, self.collected_data_counts_of_vel_steer, self.mask_vel_acc, - self.mask_vel_steer + self.mask_vel_steer, ) trajectory_longitudinal_velocity_data = np.array( diff --git a/control_data_collecting_tool/scripts/mask/mask_selector.py b/control_data_collecting_tool/scripts/mask/mask_selector.py index 639e4012..60fef14e 100644 --- a/control_data_collecting_tool/scripts/mask/mask_selector.py +++ b/control_data_collecting_tool/scripts/mask/mask_selector.py @@ -1,17 +1,29 @@ import os -import matplotlib.pyplot as plt -import matplotlib.patches as patches -import numpy as np -import threading import signal +import threading import time + +import matplotlib.patches as patches +import matplotlib.pyplot as plt +import numpy as np import yaml + class DataCollectingMaskSelector: - def __init__(self, window_title="Data Collecting Mask Selector", - x_min=0, x_max=10, y_min=-5, y_max=5, - num_bins_x=10, num_bins_y=10, default_number=1, - xlabel="X Axis", ylabel="Y Axis", mask_xy=None): + def __init__( + self, + window_title="Data Collecting Mask Selector", + x_min=0, + x_max=10, + y_min=-5, + y_max=5, + num_bins_x=10, + num_bins_y=10, + default_number=1, + xlabel="X Axis", + ylabel="Y Axis", + mask_xy=None, + ): """ Initializes the DataCollectingMaskSelector. @@ -38,7 +50,11 @@ def __init__(self, window_title="Data Collecting Mask Selector", self.ylabel = ylabel # Initialize mask_xy with the provided array or default to ones - self.mask_xy = mask_xy if mask_xy is not None else np.ones((self.num_bins_x, self.num_bins_y), dtype=int) + self.mask_xy = ( + mask_xy + if mask_xy is not None + else np.ones((self.num_bins_x, self.num_bins_y), dtype=int) + ) self.dragging = False self.select_mode = True @@ -63,7 +79,7 @@ def __init__(self, window_title="Data Collecting Mask Selector", self.ax.set_yticklabels([f"{y:.2f}" for y in self.y_bin_centers[::-1]], minor=True) # Configure grid lines and labels - self.ax.grid(True, color='black', linestyle='-', linewidth=1.5) + self.ax.grid(True, color="black", linestyle="-", linewidth=1.5) self.ax.set_title(window_title) self.ax.set_xlabel(self.xlabel) self.ax.set_ylabel(self.ylabel) @@ -89,17 +105,28 @@ def initialize_cells(self): cell_y = self.y_bin_centers[y_bin] if self.mask_xy[x_bin, y_bin] == 1: rect = patches.Rectangle( - (cell_x - (self.x_bins[1] - self.x_bins[0]) / 2, - cell_y - (self.y_bins[1] - self.y_bins[0]) / 2), + ( + cell_x - (self.x_bins[1] - self.x_bins[0]) / 2, + cell_y - (self.y_bins[1] - self.y_bins[0]) / 2, + ), self.x_bins[1] - self.x_bins[0], self.y_bins[1] - self.y_bins[0], - linewidth=1, edgecolor='yellowgreen', facecolor='yellowgreen' + linewidth=1, + edgecolor="yellowgreen", + facecolor="yellowgreen", ) self.ax.add_patch(rect) clicked_cells[(cell_x, cell_y)] = rect - text = self.ax.text(cell_x, cell_y, str(self.default_number), - ha='center', va='center', fontsize=10, color='black') + text = self.ax.text( + cell_x, + cell_y, + str(self.default_number), + ha="center", + va="center", + fontsize=10, + color="black", + ) self.text_objects[(cell_x, cell_y)] = text else: rect = None @@ -163,16 +190,26 @@ def update_cell(self, event): if cell_center not in self.clicked_cells: # Add the rectangle and text rect = patches.Rectangle( - (cell_center[0] - (self.x_bins[1] - self.x_bins[0]) / 2, - cell_center[1] - (self.y_bins[1] - self.y_bins[0]) / 2), + ( + cell_center[0] - (self.x_bins[1] - self.x_bins[0]) / 2, + cell_center[1] - (self.y_bins[1] - self.y_bins[0]) / 2, + ), self.x_bins[1] - self.x_bins[0], self.y_bins[1] - self.y_bins[0], - linewidth=1, edgecolor='yellowgreen', facecolor='yellowgreen' + linewidth=1, + edgecolor="yellowgreen", + facecolor="yellowgreen", ) self.ax.add_patch(rect) - text = self.ax.text(cell_center[0], cell_center[1], - str(self.default_number), ha='center', va='center', - fontsize=10, color='black') + text = self.ax.text( + cell_center[0], + cell_center[1], + str(self.default_number), + ha="center", + va="center", + fontsize=10, + color="black", + ) self.text_objects[cell_center] = text self.clicked_cells[cell_center] = rect self.mask_xy[x_bin, y_bin] = 1 @@ -183,7 +220,7 @@ def update_cell(self, event): del self.clicked_cells[cell_center] # Remove the text - #if cell_center in self.text_objects: + # if cell_center in self.text_objects: self.text_objects[cell_center].remove() del self.text_objects[cell_center] @@ -200,14 +237,16 @@ def load_config_from_yaml(file_path): """ if not os.path.exists(file_path): raise FileNotFoundError(f"YAML file '{file_path}' not found.") - with open(file_path, 'r') as file: + with open(file_path, "r") as file: try: return yaml.safe_load(file) except yaml.YAMLError as e: raise ValueError(f"Error parsing YAML file '{file_path}': {e}") + import numpy as np + def load_mask_from_txt(file_path): """ Loads a numerical mask from a text file into a numpy array. @@ -226,6 +265,7 @@ def load_mask_from_txt(file_path): print(f"Error loading file {file_path}: {e}") return None + def main(): """ Main function to run the mask selector application. @@ -263,26 +303,57 @@ def handle_shutdown(signum, frame): VEL_STEER_RATE_THRESHOLD = config["VEL_STEER_RATE_THRESHOLD"] # Create 3 instances of the selector with different configurations - mask_velocity_acceleration_path = os.path.join(directory_path, f"{MASK_NAME}_Velocity_Acceleration.txt") + mask_velocity_acceleration_path = os.path.join( + directory_path, f"{MASK_NAME}_Velocity_Acceleration.txt" + ) mask_velocity_acceleration = load_mask_from_txt(mask_velocity_acceleration_path) - selector1 = DataCollectingMaskSelector(window_title="Velocity-Acceleration Mask", - x_min=V_MIN, x_max=V_MAX, y_min=A_MIN, y_max=A_MAX, - num_bins_x=NUM_BINS_V, num_bins_y=NUM_BINS_A, default_number=VEL_ACC_THRESHOLD, - xlabel="Velocity (m/s)", ylabel="Acceleration (m/s^2)", mask_xy=mask_velocity_acceleration) + selector1 = DataCollectingMaskSelector( + window_title="Velocity-Acceleration Mask", + x_min=V_MIN, + x_max=V_MAX, + y_min=A_MIN, + y_max=A_MAX, + num_bins_x=NUM_BINS_V, + num_bins_y=NUM_BINS_A, + default_number=VEL_ACC_THRESHOLD, + xlabel="Velocity (m/s)", + ylabel="Acceleration (m/s^2)", + mask_xy=mask_velocity_acceleration, + ) mask_velocity_steering_path = os.path.join(directory_path, f"{MASK_NAME}_Velocity_Steering.txt") mask_velocity_steer = load_mask_from_txt(mask_velocity_steering_path) - selector2 = DataCollectingMaskSelector(window_title="Velocity-Steering Mask", - x_min=V_MIN, x_max=V_MAX, y_min=STEER_MIN, y_max=STEER_MAX, - num_bins_x=NUM_BINS_V, num_bins_y=NUM_BINS_STEER, default_number=VEL_STEER_THRESHOLD, - xlabel="Velocity (m/s)", ylabel="Steering Angle (rad)", mask_xy=mask_velocity_steer) - - mask_velocity_steering_rate_path = os.path.join(directory_path, f"{MASK_NAME}_Velocity_Steering_Rate.txt") + selector2 = DataCollectingMaskSelector( + window_title="Velocity-Steering Mask", + x_min=V_MIN, + x_max=V_MAX, + y_min=STEER_MIN, + y_max=STEER_MAX, + num_bins_x=NUM_BINS_V, + num_bins_y=NUM_BINS_STEER, + default_number=VEL_STEER_THRESHOLD, + xlabel="Velocity (m/s)", + ylabel="Steering Angle (rad)", + mask_xy=mask_velocity_steer, + ) + + mask_velocity_steering_rate_path = os.path.join( + directory_path, f"{MASK_NAME}_Velocity_Steering_Rate.txt" + ) mask_velocity_steering_rate = load_mask_from_txt(mask_velocity_steering_rate_path) - selector3 = DataCollectingMaskSelector(window_title="Velocity-Steering Rate Mask", - x_min=V_MIN, x_max=V_MAX, y_min=STEER_RATE_MIN, y_max=STEER_RATE_MAX, - num_bins_x=NUM_BINS_V, num_bins_y=NUM_BINS_STEER_RATE, default_number=VEL_STEER_RATE_THRESHOLD, - xlabel="Velocity (m/s)", ylabel="Steering Rate (rad/s)", mask_xy=mask_velocity_steering_rate) + selector3 = DataCollectingMaskSelector( + window_title="Velocity-Steering Rate Mask", + x_min=V_MIN, + x_max=V_MAX, + y_min=STEER_RATE_MIN, + y_max=STEER_RATE_MAX, + num_bins_x=NUM_BINS_V, + num_bins_y=NUM_BINS_STEER_RATE, + default_number=VEL_STEER_RATE_THRESHOLD, + xlabel="Velocity (m/s)", + ylabel="Steering Rate (rad/s)", + mask_xy=mask_velocity_steering_rate, + ) print("Press Ctrl+C to save Masks and exit.") @@ -309,14 +380,22 @@ def update_loop(selector): while not shutdown_event.is_set(): plt.pause(0.01) finally: - np.savetxt( mask_velocity_acceleration_path, selector1.mask_xy, fmt="%d") + np.savetxt(mask_velocity_acceleration_path, selector1.mask_xy, fmt="%d") print("Saved Velocity-Acceleration Mask to", mask_velocity_acceleration_path) - np.savetxt(os.path.join(directory_path, f"{MASK_NAME}_Velocity_Steering.txt"), selector2.mask_xy, fmt="%d") + np.savetxt( + os.path.join(directory_path, f"{MASK_NAME}_Velocity_Steering.txt"), + selector2.mask_xy, + fmt="%d", + ) print("Saved Velocity-Steering Mask to", mask_velocity_steering_path) - np.savetxt(os.path.join(directory_path, f"{MASK_NAME}_Velocity_Steering_Rate.txt"), selector3.mask_xy, fmt="%d") + np.savetxt( + os.path.join(directory_path, f"{MASK_NAME}_Velocity_Steering_Rate.txt"), + selector3.mask_xy, + fmt="%d", + ) print("Saved Velocity-Steering Rate Mask to", mask_velocity_steering_rate_path) print("Exiting program.") if __name__ == "__main__": - main() \ No newline at end of file + main()