diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 72470541b..3ed395a8b 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -9,9 +9,9 @@ This package provides tools for automatically collecting data using pure pursuit - This package aims to collect a dataset consisting of control inputs (i.e. `control_cmd`) and observation variables (i.e. `kinematic_state`, `steering_status`, etc). - The collected dataset can be used as training dataset for learning-based controllers, including [smart_mpc](https://github.com/autowarefoundation/autoware.universe/tree/f30c0350861d020ad26a45806ab1334895122fab/control/smart_mpc_trajectory_follower). - The data collecting approach is as follows: - - Seting a figure-eight target trajectory within the specified rectangular area. + - Setting a figure-eight target trajectory within the specified rectangular area. - Following the trajectory using a pure pursuit control law. - - Adding noises to the trajectory and the control command for data diversity, improveing the prediction accuracy of learning model. + - Adding noises to the trajectory and the control command for data diversity, improving the prediction accuracy of learning model. ## How to use @@ -101,7 +101,7 @@ ROS 2 params in `/data_collecting_pure_pursuit_trajectory_follower` node: | :--------------------------------------- | :------- | :------------------------------------------------------------- | :------------ | | `pure_pursuit_type` | `string` | Pure pursuit type (`naive` or `linearized` steer control law ) | `linearized` | | `wheel_base` | `double` | Wheel base [m] | 2.79 | -| `acc_kp` | `double` | Accel command propotional gain | 0.5 | +| `acc_kp` | `double` | Accel command proportional gain | 0.5 | | `lookahead_time` | `double` | Pure pursuit lookahead time [s] | 1.5 | | `min_lookahead` | `double` | Pure pursuit minimum lookahead length [m] | 3.0 | | `linearized_pure_pursuit_steer_kp_param` | `double` | Linearized pure pursuit steering P gain parameter | 2.0 | 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 7c1aa04b4..74e7e2485 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 @@ -40,7 +40,7 @@ def getYaw(orientation_xyzw): return R.from_quat(orientation_xyzw.reshape(-1, 4)).as_euler("xyz")[:, 2] -class DataCollectingPurePursuitTrajetoryFollower(Node): +class DataCollectingPurePursuitTrajectoryFollower(Node): def __init__(self): super().__init__("data_collecting_pure_pursuit_trajectory_follower") @@ -61,7 +61,7 @@ def __init__(self): self.declare_parameter( "acc_kp", 0.5, - ParameterDescriptor(description="Pure pursuit accel command propotional gain"), + ParameterDescriptor(description="Pure pursuit accel command proportional gain"), ) self.declare_parameter( @@ -266,8 +266,8 @@ def pure_pursuit_control( np.arctan2(pos_xy_ref_target[1] - pos_xy_obs[1], pos_xy_ref_target[0] - pos_xy_obs[0]) - pos_yaw_obs[0] ) - angz = 2.0 * longitudinal_vel_ref_nearest * np.sin(alpha) / wheel_base - steer = np.arctan(angz * wheel_base / longitudinal_vel_ref_nearest) + ang_z = 2.0 * longitudinal_vel_ref_nearest * np.sin(alpha) / wheel_base + steer = np.arctan(ang_z * wheel_base / longitudinal_vel_ref_nearest) return np.array([pure_pursuit_acc_cmd, steer]) @@ -284,7 +284,7 @@ def linearized_pure_pursuit_control( wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value acc_kp = self.get_parameter("acc_kp").get_parameter_value().double_value - # Currently, the following params are not declareed as ROS 2 params. + # Currently, the following params are not declared as ROS 2 params. lookahead_coef = self.get_parameter("lookahead_time").get_parameter_value().double_value lookahead_intercept = self.get_parameter("min_lookahead").get_parameter_value().double_value @@ -618,7 +618,7 @@ def control(self): def main(args=None): rclpy.init(args=args) - data_collecting_pure_pursuit_trajectory_follower = DataCollectingPurePursuitTrajetoryFollower() + data_collecting_pure_pursuit_trajectory_follower = DataCollectingPurePursuitTrajectoryFollower() rclpy.spin(data_collecting_pure_pursuit_trajectory_follower) 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 6b068f954..f8309218f 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -58,7 +58,7 @@ def get_trajectory_points( D = [(b - a) / 2, -a / 2] # _O = [0.0, 0.0] # origin - R = a / 2 # radious of the circle + 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 OB = np.sqrt((b - a) ** 2 + a**2) / 2 # half length of the linear trajectory @@ -271,10 +271,10 @@ def updateNominalTargetTrajectory(self): vec_from_center_to_point0_data = data_collecting_area[0, :2] - rectangle_center_position vec_from_center_to_point1_data = data_collecting_area[1, :2] - rectangle_center_position - unitvec_from_center_to_point0_data = vec_from_center_to_point0_data / ( + unit_vec_from_center_to_point0_data = vec_from_center_to_point0_data / ( np.sqrt((vec_from_center_to_point0_data**2).sum()) + 1e-10 ) - unitvec_from_center_to_point1_data = vec_from_center_to_point1_data / ( + unit_vec_from_center_to_point1_data = vec_from_center_to_point1_data / ( np.sqrt((vec_from_center_to_point1_data**2).sum()) + 1e-10 ) @@ -283,15 +283,19 @@ def updateNominalTargetTrajectory(self): if la > lb: long_side_length = la short_side_length = lb - vec_long_side = -unitvec_from_center_to_point0_data + unitvec_from_center_to_point1_data + vec_long_side = ( + -unit_vec_from_center_to_point0_data + unit_vec_from_center_to_point1_data + ) else: long_side_length = lb short_side_length = la - vec_long_side = unitvec_from_center_to_point0_data + unitvec_from_center_to_point1_data - unitvec_long_side = vec_long_side / np.sqrt((vec_long_side**2).sum()) - if unitvec_long_side[1] < 0: - unitvec_long_side *= -1 - yaw_offset = np.arccos(unitvec_long_side[0]) + vec_long_side = ( + unit_vec_from_center_to_point0_data + unit_vec_from_center_to_point1_data + ) + unit_vec_long_side = vec_long_side / np.sqrt((vec_long_side**2).sum()) + if unit_vec_long_side[1] < 0: + unit_vec_long_side *= -1 + yaw_offset = np.arccos(unit_vec_long_side[0]) if yaw_offset > pi / 2: yaw_offset -= pi