diff --git a/control_data_collecting_tool/CMakeLists.txt b/control_data_collecting_tool/CMakeLists.txt index 8039f457..f665f7a7 100644 --- a/control_data_collecting_tool/CMakeLists.txt +++ b/control_data_collecting_tool/CMakeLists.txt @@ -33,12 +33,15 @@ pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description. # Install scripts install(PROGRAMS - scripts/data_collecting_pure_pursuit_trajectory_follower.py + scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py + scripts/data_collecting_pure_pursuit_trajectory_follower_actuation_cmd.py scripts/data_collecting_trajectory_publisher.py scripts/data_collecting_plotter.py scripts/data_collecting_rosbag_record.py scripts/data_collecting_data_counter.py scripts/data_collecting_base_node.py + scripts/calibration/data_collecting_acceleration_cmd.py + scripts/calibration/data_collecting_actuation_cmd.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 16980de9..e4f0b0e9 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -62,13 +62,14 @@ This package provides tools for automatically collecting data using pure pursuit 4. Launch control_data_collecting_tool. ```bash - ros2 launch control_data_collecting_tool control_data_collecting_tool.launch.py map_path:=$HOME/autoware_map/sample-map-planning + ros2 launch control_data_collecting_tool control_data_collecting_tool.launch.py map_path:=$HOME/autoware_map/sample-map-planning accel_brake_map_path:=/path/to/your/accel_brake_map_dir ``` - If you use the `along_road` course, please specify the same map for `map_path` as the one used when launching Autoware. `map_path` is not necessary when using courses other than `along_road`. - - Control data collecting tool automatically records topics included in `config/topics.yaml` when the above command is executed. - Topics will be saved in rosbag2 format in the current directory. + - If you set CONTROL_MODE to actuation_cmd or external_actuation_cmd, please specify the directory where the accel/brake maps used by your control system are located. + + - Control data collecting tool automatically records topics included in `config/topics.yaml` when the above command is executed. Topics will be saved in rosbag2 format in the current directory. - The data from `/localization/kinematic_state` and `/localization/acceleration` located in the directory (rosbag2 format) where the command is executed will be automatically loaded and reflected in the data count for these topics. (If `LOAD_ROSBAG2_FILES` in `config/param.yaml` is set to `false`, the data is not loaded.) @@ -112,21 +113,207 @@ This package provides tools for automatically collecting data using pure pursuit > You cannot change the goal pose while driving. > In cases where course generation fails, which can happen under certain conditions, please reposition the vehicle or redraw the goal pose. -7. Click the `LOCAL` button in `AutowareStatePanel`. +7. The following actions differ depending on the `CONTROL_MODE`. If you select the control mode from [ `acceleration_cmd`], please proceed to 7.1. If you select the control mode from [`actuation_cmd`], please proceed to 7.2. If you select the control mode from [`external_acceleration_cmd`, `external_actuation_cmd`], please proceed to 7.3. + + - 7.1 If you choose the control mode from [ `acceleration_cmd`], click the `LOCAL` button in `AutowareStatePanel`. + + + + Then, data collecting starts. + + + + You can monitor the data collection status in real-time through the window that pops up when this node is launched. + (From top to bottom: the speed-acceleration phase diagram, the speed-acceleration heatmap, the speed-steering angle heatmap, the speed-steer rate heatmap, and the speed-jerk heatmap.) + + + + For the speed-acceleration heatmap, speed-steering angle heatmap, and speed-steer rate heatmap, the collection range can be specified by the masks located in the folder `config/masks/MASK_NAME` where `MASK_NAME` is a parameter specifying mask name (Please also see `config/common_param.yaml`). + The specified heatmap cells are designed to change from blue to green once a certain amount of data (`VEL_ACC_THRESHOLD`, `VEL_STEER_THRESHOLD`, `VEL_ABS_STEER_RATE_THRESHOLD` ) is collected. It is recommended to collect data until as many cells as possible turn green. + + - 7.2 If you choose the control mode from [`actuation_cmd`], click the LOCAL button in the AutowareStatePanel as described in Section 7.1. + > [NOTE] + > At this time, the control mode `actuation_cmd` is only implemented in the course `reversal_loop_circle` and cannot be used in other courses. + + This mode allows you to collect data on various accel and brake pedal inputs. To monitor the data collection status of accel/brake input, please use the functionality of [autoware_accel_brake_map_calibrator](https://github.com/autowarefoundation/autoware.universe/blob/main/vehicle/autoware_accel_brake_map_calibrator/README.md). + + - 7.3 In the case you choose the control mode from [`external_acceleration_cmd`, `external_actuation_cmd`]. + + - `external_acceleration_cmd` + + This mode enables the collection of constant acceleration data for both positive and negative acceleration scenarios. + + - Positive Acceleration Data Collection + + a. Start the Tool by using the following command + + ```bash + ros2 run control_data_collecting_tool data_collecting_acceleration_cmd.py + ``` + + b. Confirm Data Collection: The tool prompts you to confirm whether to proceed with positive acceleration data collection: + + ```bash + Do you want to collect constant positive acceleration cmd data? (yes/no) + ``` + + c. Input Acceleration: When prompted, input the desired acceleration value: + + ```bash + Target acceleration [0.0 ~ 2.0 m/s^2] + ``` + + d. Gear Change and Readiness Check: Pleas click the `LOCAL` button in `AutowareStatePanel` as in 7.1 and the tool checks if the system is ready for the operation. You will see the following prompt: + + ```bash + Ready to drive? (yes/no) + ``` + + f. Execution and Recording: After 3-second counting, the vehicle accelerates to `TARGET_VELOCITY` specified in the configuration file. During this process, the following message is displayed: + + ```bash + Accelerate with target_acceleration m/s^2 + ``` + + A ROS bag file records all relevant topics during this session. The filename is generated as constant_acceleration_cmd_ACCEL_TARGET_ACCELERATION_CURRENT_TIME, where TARGET_ACCELERATION is input target acceleration value, and CURRENT_TIME is the timestamp in YYYYMMDD-HHMMSS format. The ROS bag file will specifically include topics matching the regular expression: + + ```bash + /control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*) + ``` + + g. Deceleration Phase: After reaching `TARGET_VELOCITY`, the tool applies a deceleration using the TARGET_ACCELERATION_FOR_BRAKE parameter to bring the vehicle to a stop. + + h. Completion: Once the data is recorded and the vehicle is safely stopped, the session ends. The tool validates the recorded data. + + - Negative Acceleration Data Collection + + a. Start the Tool by using the following command + + ```bash + ros2 run control_data_collecting_tool data_collecting_acceleration_cmd + ``` + + b. Confirm Data Collection: The tool prompts you to confirm whether to proceed with negative acceleration data collection (the following message will be displayed after answering `no` to `Do you want to collect constant positive acceleration cmd data? (yes/no)` in Positive Acceleration Data Collection): + + ```bash + Do you want to collect constant negative acceleration cmd data? (yes/no) + ``` + + c. Input Acceleration: When prompted, input the desired acceleration value: + + ```bash + Target acceleration [-5.0 ~ 0.0 m/s^2] + ``` + + d. Gear Change and Readiness Check: Pleas click the `LOCAL` button in `AutowareStatePanel` as in 7.1 and the tool checks if the system is ready for the operation. You will see the following prompt: - + ```bash + Ready to drive? (yes/no) + ``` - Then, data collecting starts. + e. Acceleration Phase: After 3-second counting, the vehicle accelerates to the `TARGET_VELOCITY` with TARGET_ACCELERATION_FOR_DRIVE before braking. - + f. Braking and Recording: Once `TARGET_VELOCITY` is achieved, the tool applies the braking command. During this process, you will see: - You can monitor the data collection status in real-time through the window that pops up when this node is launched. - (From top to bottom: the speed-acceleration phase diagram, the speed-acceleration heatmap, the speed-steering angle heatmap, the speed-steer rate heatmap, and the speed-jerk heatmap.) + ```bash + Accelerate with target_acceleration m/s^2 + ``` - + A ROS bag file records all relevant topics during this session. The filename is generated as constant_acceleration_cmd_BRAKE_TARGET_ACCELERATION_CURRENT_TIME, where TARGET_ACCELERATION is input target acceleration value, and CURRENT_TIME is the timestamp in YYYYMMDD-HHMMSS format. The ROS bag file will specifically include topics matching the regular expression: - For the speed-acceleration heatmap, speed-steering angle heatmap, and speed-steer rate heatmap, the collection range can be specified by the masks located in the folder `config/masks/MASK_NAME` where `MASK_NAME` is a parameter specifying mask name (Please also see `config/common_param.yaml`). - The specified heatmap cells are designed to change from blue to green once a certain amount of data (`VEL_ACC_THRESHOLD`, `VEL_STEER_THRESHOLD`, `VEL_ABS_STEER_RATE_THRESHOLD` ) is collected. It is recommended to collect data until as many cells as possible turn green. + ```bash + /control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*) + ``` + + g. Completion: Once the data is recorded and the vehicle is safely stopped, the session ends. The tool validates the recorded data. + + - `external_actuation_cmd` + + This mode enables the collection of constant accel/brake pedal input data. + + - Accel Pedal Data Collection + + a. Start the tool by using the following command + + ```bash + ros2 run control_data_collecting_tool data_collecting_actuation_cmd + ``` + + b. Confirm Data Collection: The tool prompts you to confirm whether to proceed with positive acceleration data collection: + + ```bash + Do you want to accel pedal input data? (yes/no) + ``` + + c. Input Accel Pedal input: When prompted, input the desired accel pedal input value: + + ```bash + Target accel pedal input [0.0 ~ 0.5 ] + ``` + + d. Gear Change and Readiness Check: Pleas click the `LOCAL` button in `AutowareStatePanel` as in 7.1 and the tool checks if the system is ready for the operation. You will see the following prompt: + + ```bash + Ready to drive? (yes/no) + ``` + + e. Execution and Recording: After 3-second counting, the vehicle accelerates to `TARGET_VELOCITY` specified in the configuration file. During this process, the following message is displayed: + + ```bash + Actuate with accel pedal input: target_accel_pedal_input + ``` + + A ROS bag file records all relevant topics during this session. The filename is generated as constant_actuation_cmd_ACCEL_TARGET_ACCELERATION_CURRENT_TIME, where TARGET_ACCELERATION is input target acceleration value, and CURRENT_TIME is the timestamp in YYYYMMDD-HHMMSS format. The ROS bag file will specifically include topics matching the regular expression: + + ```bash + /control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*) + ``` + + f. Deceleration Phase: After reaching `TARGET_VELOCITY`, the tool applies a deceleration using the TARGET_ACTUATION_FOR_BRAKE parameter to bring the vehicle to a stop. + + g. Completion: Once the data is recorded and the vehicle is safely stopped, the session ends. The tool validates the recorded data. + + - Brake Pedal Data Collection + + a. Start the tool by using the following command + + ```bash + ros2 run control_data_collecting_tool data_collecting_actuation_cmd + ``` + + b. Confirm Data Collection: The tool prompts you to confirm whether to proceed with brake pedal input data collection (the following message will be displayed after answering `no` to `Do you want to accel pedal input data? (yes/no)` in Accel Pedal Data Collection): + + ```bash + Do you want to brake pedal input data? (yes/no) + ``` + + c. Input Brake Pedal input: When prompted, input the desired brake pedal input value: + + ```bash + Target brake pedal input [0.0 ~ 0.8 ] + ``` + + d. Gear Change and Readiness Check: Pleas click the `LOCAL` button in `AutowareStatePanel` as in 7.1 and the tool checks if the system is ready for the operation. You will see the following prompt: + + ```bash + Ready to drive? (yes/no) + ``` + + e. Acceleration Phase: After 3-second counting, the vehicle accelerates to the `TARGET_VELOCITY` with TARGET_ACTUATION_FOR_ACCEL before braking. + + f. Braking and Recording: Once `TARGET_VELOCITY` is achieved, the tool applies the braking command. During this process, you will see: + + ```bash + Actuate with brake pedal input: target_brake_pedal_input + ``` + + A ROS bag file records all relevant topics during this session. The filename is generated as constant_actuation_cmd_ACCEL_TARGET_ACCELERATION_CURRENT_TIME, where TARGET_ACCELERATION is input target acceleration value, and CURRENT_TIME is the timestamp in YYYYMMDD-HHMMSS format. The ROS bag file will specifically include topics matching the regular expression: + + ```bash + /control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*) + ``` + + g. Completion: Once the data is recorded and the vehicle is safely stopped, the session ends. The tool validates the recorded data. 8. If you want to stop data collecting automatic driving, run the following command @@ -197,8 +384,8 @@ You can create an original mask to specify the data collection range for the hea 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 + # b is constant variable and sine_curve is derived from appropriate amplitude and period, defined separately + target_velocity = current_velocity + abs(target_acceleration_on_section) / acc_kp * (b + 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. @@ -207,16 +394,16 @@ You can create an original mask to specify the data collection range for the hea 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 + # b is constant variable and sine_curve is derived from appropriate amplitude and period, defined separately + target_velocity = target_velocity_on_section + b + 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 + # b is constant variable and sine_curve is derived from appropriate amplitude and period, defined separately + target_velocity = current_velocity - abs(target_acceleration_on_section) / acc_kp * (b + sine_curve) ``` After decelerating to a sufficiently low speed, return to step i. @@ -267,7 +454,7 @@ You can create an original mask to specify the data collection range for the hea 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. + 1. Starting from the ego vehicle's current position, the system examines a segment of the trajectory ahead, covering a distance defined by look_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. @@ -275,14 +462,13 @@ You can create an original mask to specify the data collection range for the hea ## Parameter -There are parameters that are common to all trajectories and parameters that are specific to each trajectory. - ### Common Parameters ROS 2 parameters which are common in all trajectories (`/config/common_param.yaml`): | Name | Type | Description | Default value | | :--------------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------- | :--------------------- | +| `CONTROL_MODE` | `string` | Control mode [`acceleration_cmd`, `actuation_cmd`, `external_acceleration_cmd`, `external_actuation_cmd`] | `acceleration_cmd` | | `LOAD_ROSBAG2_FILES` | `bool` | Flag that determines whether to load rosbag2 data or not | true | | `COURSE_NAME` | `string` | Course name [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`, `reversal_loop_circle`, `along_road`] | `reversal_loop_circle` | | `NUM_BINS_V` | `int` | Number of bins of velocity in heatmap | 10 | @@ -290,6 +476,8 @@ ROS 2 parameters which are common in all trajectories (`/config/common_param.yam | `NUM_BINS_A` | `int` | Number of bins of acceleration in heatmap | 10 | | `NUM_BINS_ABS_STEER_RATE` | `int` | Number of bins of absolute value of steer rate in heatmap | 5 | | `NUM_BINS_JERK` | `int` | Number of bins of jerk in heatmap | 10 | +| `NUM_BINS_ACCEL_PEDAL_INPUT` | `int` | Number of bins of accel pedal input in heatmap | 8 | +| `NUM_BINS_BRAKE_PEDAL_INPUT` | `int` | Number of bins of brake pedal input in heatmap | 16 | | `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] | -0.6 | @@ -301,6 +489,11 @@ ROS 2 parameters which are common in all trajectories (`/config/common_param.yam | `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 | | `JERK_MAX` | `double` | Maximum jerk in heatmap [m/s^3] | 0.5 | +| `ACCEL_PEDAL_INPUT_MIN` | `double` | Minimum accel pedal in heatmap | 0.4 | +| `ACCEL_PEDAL_INPUT_MAX` | `double` | Maximum accel pedal in heatmap | 0.0 | +| `BRAKE_PEDAL_INPUT_MIN` | `double` | Minimum brake pedal in heatmap | 0.8 | +| `BRAKE_PEDAL_INPUT_MAX` | `double` | Maximum brake pedal in heatmap | 0.0 | +| `STEER_THRESHOLD_FOR_PEDAL_INPUT_COUNT` | `string` | Threshold of steering angle to count pedal input data | 0.2 | | `MASK_NAME` | `string` | Directory name of masks for data collection | `default` | | `VEL_ACC_THRESHOLD` | `int` | Threshold of velocity-and-acc heatmap in data collection | 40 | | `VEL_STEER_THRESHOLD` | `int` | Threshold of velocity-and-steer heatmap in data collection | 20 | @@ -385,3 +578,27 @@ Each trajectory has specific ROS 2 parameters. | `minimum_length_of_straight_line` | `double` | The minimum length of straight line for data collection [m] | 50.0 | | `longitude` | `double` | The longitude of the origin specified when loading the map [degree] | 139.6503 | | `latitude` | `double` | The latitude of the origin specified when loading the map [degree] | 35.6762 | + +### Parameters for `data_collecting_acceleration_cmd.py` and `data_collecting_actuation_cmd.py` + +- `data_collecting_acceleration_cmd.py` + +| Name | Type | Description | Default value | +| :------------------------------ | :------- | :-------------------------------------------------------------------------- | :------------ | +| `TARGET_VELOCITY` | `double` | The maximum velocity for data collection [m/s] | 11.80 | +| `TARGET_ACCELERATION_FOR_DRIVE` | `double` | Target acceleration when collecting deceleration data [m/s^2] | 0.3 | +| `TARGET_ACCELERATION_FOR_BRAKE` | `double` | Target deceleration when collecting acceleration data [m/s^2] | 0.5 | +| `TARGET_JERK_FOR_DRIVE` | `double` | The target rate of change of acceleration (jerk) for smooth driving [m/s^3] | 1.5 | +| `TARGET_JERK_FOR_BRAKE` | `double` | The target rate of change of acceleration (jerk) when braking [m/s^3] | -1.5 | +| `MIN_ACCEL` | `double` | The minimum allowable acceleration for data collection [m/s^2] | -5.0 | +| `MAX_ACCEL` | `double` | The maximum allowable acceleration for data collection [m/s^2] | 2.0 | + +- `data_collecting_actuation_cmd.py` + +| Name | Type | Description | Default value | +| :--------------------------- | :------- | :-------------------------------------------------------------- | :------------ | +| `TARGET_VELOCITY` | `double` | The maximum velocity for data collection [m/s] | 11.80 | +| `TARGET_ACTUATION_FOR_ACCEL` | `double` | Target accel pedal input when collecting brake pedal input data | 0.3 | +| `TARGET_ACTUATION_FOR_BRAKE` | `double` | Target brake pedal input when collecting accel pedal input data | 0.5 | +| `MAX_ACCEL_PEDAL` | `double` | The maximum allowable accel pedal input for data collection | 0.5 | +| `MIN_BRAKE_PEDAL` | `double` | The maximum allowable brake pedal input for data collection | 0.8 | diff --git a/control_data_collecting_tool/config/common_param.yaml b/control_data_collecting_tool/config/common_param.yaml index 3e5c12c4..2e076e3d 100644 --- a/control_data_collecting_tool/config/common_param.yaml +++ b/control_data_collecting_tool/config/common_param.yaml @@ -1,6 +1,11 @@ /**: ros__parameters: - LOAD_ROSBAG2_FILES: true + CONTROL_MODE: acceleration_cmd + # CONTROL_MODE: actuation_cmd + # CONTROL_MODE: external_acceleration_cmd + # CONTROL_MODE: external_actuation_cmd + + LOAD_ROSBAG2_FILES: false # COURSE_NAME: eight_course # COURSE_NAME: u_shaped_return @@ -14,6 +19,8 @@ NUM_BINS_A: 10 NUM_BINS_ABS_STEER_RATE: 5 NUM_BINS_JERK: 10 + NUM_BINS_ACCEL_PEDAL_INPUT: 8 + NUM_BINS_BRAKE_PEDAL_INPUT: 16 V_MIN: 0.0 V_MAX: 11.5 STEER_MIN: -0.6 @@ -24,6 +31,12 @@ ABS_STEER_RATE_MAX: 0.3 JERK_MIN: -0.5 JERK_MAX: 0.5 + ACCEL_PEDAL_INPUT_MAX: 0.4 + ACCEL_PEDAL_INPUT_MIN: 0.0 + BRAKE_PEDAL_INPUT_MAX: 0.8 + BRAKE_PEDAL_INPUT_MIN: 0.0 + + STEER_THRESHOLD_FOR_PEDAL_INPUT_COUNT: 0.2 MASK_NAME: default VEL_ACC_THRESHOLD: 40 diff --git a/control_data_collecting_tool/config/constant_cmd_param/acceleration_cmd_param.yaml b/control_data_collecting_tool/config/constant_cmd_param/acceleration_cmd_param.yaml new file mode 100644 index 00000000..b485f06f --- /dev/null +++ b/control_data_collecting_tool/config/constant_cmd_param/acceleration_cmd_param.yaml @@ -0,0 +1,17 @@ +/data_collecting_acceleration_cmd: + ros__parameters: + # params for data_collecting_acceleration_cmd + TARGET_VELOCITY: 11.80 + TARGET_ACCELERATION_FOR_DRIVE: 1.5 + TARGET_ACCELERATION_FOR_BRAKE: -1.5 + TARGET_JERK_FOR_DRIVE: 1.5 + TARGET_JERK_FOR_BRAKE: -1.5 + MIN_ACCEL: -5.0 + MAX_ACCEL: 2.0 + topics: + - /vehicle/status/velocity_status + - /control/command/actuation_cmd + - /sensing/imu/imu_data + - /vehicle/status/control_mode + validation_nodes: + - /data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd diff --git a/control_data_collecting_tool/config/constant_cmd_param/actuation_cmd_param.yaml b/control_data_collecting_tool/config/constant_cmd_param/actuation_cmd_param.yaml new file mode 100644 index 00000000..a4ab14c4 --- /dev/null +++ b/control_data_collecting_tool/config/constant_cmd_param/actuation_cmd_param.yaml @@ -0,0 +1,16 @@ +/data_collecting_actuation_cmd: + ros__parameters: + # params for data_collecting_actuation_cmd + TARGET_VELOCITY: 11.80 + TARGET_ACTUATION_FOR_ACCEL: 0.3 + TARGET_ACTUATION_FOR_BRAKE: 0.5 + MAX_ACCEL_PEDAL: 0.5 + MIN_BRAKE_PEDAL: 0.8 + topics: + - /vehicle/status/velocity_status + - /control/command/control_cmd + - /sensing/imu/imu_data + - /vehicle/status/control_mode + validation_nodes: + - /raw_vehicle_cmd_converter + - /data_collecting_pure_pursuit_trajectory_follower_actuation_cmd diff --git a/control_data_collecting_tool/config/topics.yaml b/control_data_collecting_tool/config/topics.yaml index 6fc3ec9a..7679564a 100644 --- a/control_data_collecting_tool/config/topics.yaml +++ b/control_data_collecting_tool/config/topics.yaml @@ -2,9 +2,7 @@ topics: - /localization/kinematic_state - /localization/acceleration - /vehicle/status/steering_status - - /sensing/imu/imu_data - /system/operation_mode/state - - /vehicle/status/control_mode + - /control/command/control_cmd - /external/selected/control_cmd - - /external/selected/gear_cmd - - /data_collecting_trajectory + - /control/command/actuation_cmd 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 bfdcf910..0edd4755 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 @@ -35,6 +35,15 @@ def get_course_name(yaml_file_path): return course_name +def get_control_mode(yaml_file_path): + with open(yaml_file_path, "r") as file: + data = yaml.safe_load(file) + + # get 'COURSE_NAME' + control_mode = data.get("/**", {}).get("ros__parameters", {}).get("CONTROL_MODE", None) + return control_mode + + def generate_launch_description(): # Define the argument for map_path map_path_arg = DeclareLaunchArgument( @@ -45,6 +54,14 @@ def generate_launch_description(): # Use the map_path argument in parameters map_path = LaunchConfiguration("map_path") + # Define the path to the accel/brake map file + map_path_arg = DeclareLaunchArgument( + "accel_brake_map_path", + description="Path to the accel/brake map directory (optional; defaults to None if not provided).", + default_value="", # Default to None + ) + accel_brake_map_path = LaunchConfiguration("accel_brake_map_path") + # Get the path to the common param file package_share_directory = get_package_share_directory("control_data_collecting_tool") common_param_file_path = os.path.join(package_share_directory, "config", "common_param.yaml") @@ -55,44 +72,91 @@ def generate_launch_description(): package_share_directory, "config/course_param", course_name + "_param.yaml" ) - return launch.LaunchDescription( - [ - map_path_arg, - Node( - package="control_data_collecting_tool", - executable="data_collecting_pure_pursuit_trajectory_follower.py", - name="data_collecting_pure_pursuit_trajectory_follower", - parameters=[common_param_file_path], - ), - Node( - package="control_data_collecting_tool", - executable="data_collecting_trajectory_publisher.py", - name="data_collecting_trajectory_publisher", - parameters=[ - common_param_file_path, - course_specific_param_file_path, - {"map_path": map_path}, - ], - ), - Node( - package="control_data_collecting_tool", - executable="data_collecting_plotter.py", - name="data_collecting_plotter", - parameters=[common_param_file_path], - ), - Node( - package="control_data_collecting_tool", - executable="data_collecting_rosbag_record.py", - name="data_collecting_rosbag_record", - ), - Node( - package="control_data_collecting_tool", - executable="data_collecting_data_counter.py", - name="data_collecting_data_counter", - parameters=[common_param_file_path], - ), - ] - ) + # Get the control mode + control_mode = get_control_mode(common_param_file_path) + + if control_mode == "acceleration_cmd" or control_mode == "external_acceleration_cmd": + return launch.LaunchDescription( + [ + map_path_arg, + Node( + package="control_data_collecting_tool", + executable="data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py", + name="data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd", + parameters=[common_param_file_path], + ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_trajectory_publisher.py", + name="data_collecting_trajectory_publisher", + parameters=[ + common_param_file_path, + course_specific_param_file_path, + {"map_path": map_path}, + ], + ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_plotter.py", + name="data_collecting_plotter", + parameters=[common_param_file_path], + ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_rosbag_record.py", + name="data_collecting_rosbag_record", + ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_data_counter.py", + name="data_collecting_data_counter", + parameters=[common_param_file_path], + ), + ] + ) + + elif control_mode == "actuation_cmd" or control_mode == "external_actuation_cmd": + return launch.LaunchDescription( + [ + map_path_arg, + Node( + package="control_data_collecting_tool", + executable="data_collecting_pure_pursuit_trajectory_follower_actuation_cmd.py", + name="data_collecting_pure_pursuit_trajectory_follower_actuation_cmd", + parameters=[ + common_param_file_path, + {"accel_brake_map_path": accel_brake_map_path}, + ], + ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_trajectory_publisher.py", + name="data_collecting_trajectory_publisher", + parameters=[ + common_param_file_path, + course_specific_param_file_path, + {"map_path": map_path}, + ], + ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_plotter.py", + name="data_collecting_plotter", + parameters=[common_param_file_path], + ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_rosbag_record.py", + name="data_collecting_rosbag_record", + ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_data_counter.py", + name="data_collecting_data_counter", + parameters=[common_param_file_path], + ), + ] + ) if __name__ == "__main__": diff --git a/control_data_collecting_tool/resource/looking_ahead.png b/control_data_collecting_tool/resource/looking_ahead.png index 10d0dcad..3c098291 100644 Binary files a/control_data_collecting_tool/resource/looking_ahead.png and b/control_data_collecting_tool/resource/looking_ahead.png differ diff --git a/control_data_collecting_tool/scripts/accel_brake_map.py b/control_data_collecting_tool/scripts/accel_brake_map.py new file mode 100644 index 00000000..61cb1a9e --- /dev/null +++ b/control_data_collecting_tool/scripts/accel_brake_map.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Proxima Technology Inc, TIER IV +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np +import pandas as pd +from scipy.interpolate import RegularGridInterpolator + + +class MapConverter: + def __init__(self, path_to_map): + # Load map data from CSV and initialize grid values + map_df = pd.read_csv(path_to_map, delimiter=",", header=0) + self.map_grid = map_df.to_numpy()[:, 1:] + + # Extract velocity grid from column headers + self.velocity_grid_map = np.array(map_df.columns[1:], dtype=float) + self.max_vel_of_map = self.velocity_grid_map[-1] + self.min_vel_of_map = self.velocity_grid_map[0] + + # Extract pedal grid from the first column + self.pedal_grid_map = map_df.to_numpy()[:, 0] + self.max_pedal_of_map = self.pedal_grid_map[-1] + self.min_pedal_of_map = self.pedal_grid_map[0] + + # Create an interpolator for the map grid + self.map_grid_interpolator = RegularGridInterpolator( + (self.pedal_grid_map, self.velocity_grid_map), self.map_grid + ) + + def pedal_to_accel_input(self, pedal, velocity): + # Convert pedal input and velocity to acceleration input using interpolation + pedal = np.clip(pedal, self.min_pedal_of_map, self.max_pedal_of_map) + velocity = np.clip(velocity, self.min_vel_of_map, self.max_vel_of_map) + accel_input = self.map_grid_interpolator([pedal, velocity])[0] + + return accel_input + + def accel_input_to_pedal(self, accel_input, velocity): + # Convert acceleration input and velocity to pedal input + velocity = np.clip(velocity, self.min_vel_of_map, self.max_vel_of_map) + + # Find velocity interval for interpolation + vel_idx = 0 + alpha = 0.0 + for i in range(len(self.velocity_grid_map) - 1): + vel_left_grid = self.velocity_grid_map[i] + vel_right_grid = self.velocity_grid_map[i + 1] + + if vel_left_grid <= velocity <= vel_right_grid: + vel_idx = i + alpha = (velocity - vel_left_grid) / (vel_right_grid - vel_left_grid) + + # Interpolate acceleration input map for given velocity + accel_input_map = ( + alpha * self.map_grid[:, vel_idx] + (1 - alpha) * self.map_grid[:, vel_idx + 1] + ) + + # Clip acceleration input within the bounds of the map + min_accel_map = np.min([accel_input_map[0], accel_input_map[-1]]) + max_accel_map = np.max([accel_input_map[0], accel_input_map[-1]]) + accel_input = np.clip(accel_input, min_accel_map, max_accel_map) + + # Find corresponding pedal input using interpolation + actuation_input = 0.0 + for i in range(len(accel_input_map) - 1): + if accel_input_map[i] <= accel_input_map[i + 1]: + # Increasing interval + if accel_input_map[i] <= accel_input <= accel_input_map[i + 1]: + delta_accel_input = accel_input_map[i + 1] - accel_input_map[i] + if abs(delta_accel_input) < 1e-3: + actuation_input = self.pedal_grid_map[i] + else: + beta = (accel_input - accel_input_map[i]) / delta_accel_input + actuation_input = ( + beta * self.pedal_grid_map[i + 1] + (1 - beta) * self.pedal_grid_map[i] + ) + elif accel_input_map[i + 1] < accel_input_map[i]: + # Decreasing interval + sign = -1 + if accel_input_map[i + 1] <= accel_input <= accel_input_map[i]: + delta_accel_input = accel_input_map[i] - accel_input_map[i + 1] + if abs(delta_accel_input) < 1e-3: + actuation_input = sign * self.pedal_grid_map[i] + else: + beta = (accel_input - accel_input_map[i + 1]) / delta_accel_input + actuation_input = sign * ( + beta * self.pedal_grid_map[i] + (1 - beta) * self.pedal_grid_map[i + 1] + ) + + return actuation_input + + +class AccelBrakeMapConverter: + def __init__(self, path_to_accel_map, path_to_brake_map): + # Initialize converters for acceleration and brake maps + self.accel_map_converter = MapConverter(path_to_accel_map) + self.brake_map_converter = MapConverter(path_to_brake_map) + + def convert_accel_input_to_actuation_cmd(self, accel_input, velocity): + # Convert acceleration input to actuation command + accel_actuation_cmd = self.accel_map_converter.accel_input_to_pedal(accel_input, velocity) + brake_actuation_cmd = self.brake_map_converter.accel_input_to_pedal(accel_input, velocity) + + return accel_actuation_cmd - brake_actuation_cmd + + def convert_actuation_cmd_to_accel_input(self, actuation_cmd, velocity): + # Convert actuation command to acceleration input + accel_input = 0.0 + if actuation_cmd > 0.0: + # Positive command corresponds to acceleration + accel_input = self.accel_map_converter.pedal_to_accel_input( + abs(actuation_cmd), velocity + ) + if actuation_cmd <= 0.0: + # Negative command corresponds to braking + accel_input = self.brake_map_converter.pedal_to_accel_input( + abs(actuation_cmd), velocity + ) + + return accel_input diff --git a/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py b/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py new file mode 100755 index 00000000..39d521fa --- /dev/null +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py @@ -0,0 +1,245 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from datetime import datetime +import os +import sys + +from ament_index_python.packages import get_package_share_directory +from autoware_control_msgs.msg import Control +from autoware_vehicle_msgs.msg import ControlModeReport +from autoware_vehicle_msgs.msg import GearCommand +from autoware_vehicle_msgs.msg import GearReport +from autoware_vehicle_msgs.msg import VelocityReport +from autoware_vehicle_msgs.srv import ControlModeCommand +import lib.command +import lib.cui +import lib.rosbag +import lib.system +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32 +from tier4_vehicle_msgs.msg import ActuationCommandStamped +import yaml + +COUNTDOWN_TIME = 3 # [sec] + + +class DataCollectingAccelerationCmd(Node): + def __init__(self): + super().__init__("data_collecting_acceleration_cmd") + + package_share_directory = get_package_share_directory("control_data_collecting_tool") + topic_file_path = os.path.join( + package_share_directory, "config/constant_cmd_param", "acceleration_cmd_param.yaml" + ) + with open(topic_file_path, "r") as file: + topic_data = yaml.safe_load(file) + + self.TARGET_VELOCITY = topic_data["/data_collecting_acceleration_cmd"]["ros__parameters"][ + "TARGET_VELOCITY" + ] + self.TARGET_ACCELERATION_FOR_DRIVE = topic_data["/data_collecting_acceleration_cmd"][ + "ros__parameters" + ]["TARGET_ACCELERATION_FOR_DRIVE"] + self.TARGET_ACCELERATION_FOR_BRAKE = topic_data["/data_collecting_acceleration_cmd"][ + "ros__parameters" + ]["TARGET_ACCELERATION_FOR_BRAKE"] + self.TARGET_JERK_FOR_DRIVE = topic_data["/data_collecting_acceleration_cmd"][ + "ros__parameters" + ]["TARGET_JERK_FOR_DRIVE"] + self.TARGET_JERK_FOR_BRAKE = topic_data["/data_collecting_acceleration_cmd"][ + "ros__parameters" + ]["TARGET_JERK_FOR_BRAKE"] + self.MIN_ACCEL = topic_data["/data_collecting_acceleration_cmd"]["ros__parameters"][ + "MIN_ACCEL" + ] + self.MAX_ACCEL = topic_data["/data_collecting_acceleration_cmd"]["ros__parameters"][ + "MAX_ACCEL" + ] + self.TOPIC_LIST_FOR_VALIDATION = topic_data["/data_collecting_acceleration_cmd"][ + "ros__parameters" + ]["topics"] + self.NODE_LIST_FOR_VALIDATION = topic_data["/data_collecting_acceleration_cmd"][ + "ros__parameters" + ]["validation_nodes"] + + self.client_control_mode = self.create_client( + ControlModeCommand, "/control/control_mode_request" + ) + + while not self.client_control_mode.wait_for_service(timeout_sec=1.0): + print("Waiting for the control mode service to become available...") + + self.pub_data_collecting_control_cmd = self.create_publisher( + Float32, "/data_collecting_accel_cmd", 1 + ) + self.pub_control_cmd = self.create_publisher(Control, "/control/command/control_cmd", 1) + self.pub_gear_cmd = self.create_publisher(GearCommand, "/control/command/gear_cmd", 1) + + self.sub_velocity_status = self.create_subscription( + VelocityReport, "/vehicle/status/velocity_status", self.on_velocity_status, 1 + ) + self.sub_control_mode = self.create_subscription( + ControlModeReport, "/vehicle/status/control_mode", self.on_control_mode, 1 + ) + self.sub_gear_status = self.create_subscription( + GearReport, "/vehicle/status/gear_status", self.on_gear_status, 1 + ) + + self.current_velocity = 0.0 + self.current_control_mode = ControlModeReport.MANUAL + self.current_gear = GearReport.NONE + + # For commands reset + self.pub_data_collecting_pedal_input = self.create_publisher( + Float32, "/data_collecting_pedal_input", 1 + ) + self.pub_actuation_cmd = self.create_publisher( + ActuationCommandStamped, "/control/command/actuation_cmd", 1 + ) + + def on_velocity_status(self, msg): + self.current_velocity = msg.longitudinal_velocity + + def on_control_mode(self, msg): + self.current_control_mode = msg.mode + + def on_gear_status(self, msg): + self.current_gear = msg.report + + def run(self): + print("===== Start data_collecting_acceleration_cmd =====") + lib.system.check_service_active("autoware.service") + lib.system.check_node_active(self.NODE_LIST_FOR_VALIDATION) + + print("===== Start collecting constant positive acceleration cmd data =====") + lib.cui.do_check( + "Do you want to collect constant positive acceleration cmd data?", + lambda: self.check("accel"), + ) + + print("===== Start collecting constant negative acceleration cmd data =====") + lib.cui.do_check( + "Do you want to collect constant negative acceleration cmd data?", + lambda: self.check("brake"), + ) + + print("===== Successfully finished! =====") + + def check(self, mode): + is_finished = False + while not is_finished: + print("===== Input target acceleration =====") + min_acceleration, max_acceleration = self.get_min_max_acceleration(mode) + target_acceleration = lib.cui.input_target_value( + "acceleration", min_acceleration, max_acceleration, "m/s^2" + ) + + if mode == "accel": + print( + f"===== Drive to {self.TARGET_VELOCITY} m/s with acceleration {target_acceleration} =====" + ) + lib.command.change_gear(self, "drive") + lib.cui.ready_check("Ready to drive?") + lib.cui.countdown(COUNTDOWN_TIME) + print("===== Record rosbag =====") + filename = self.get_rosbag_name(mode, target_acceleration) + process = lib.rosbag.record_ros2_bag(filename, lib.rosbag.TOPIC_LIST) + print(f"record rosbag: {filename}") + lib.command.accelerate( + self, target_acceleration, self.TARGET_VELOCITY, "drive", break_time=60.0 + ) + print("===== End rosbag record =====") + process.terminate() + lib.command.accelerate( + self, + self.TARGET_ACCELERATION_FOR_BRAKE, + 1e-3, + "brake", + self.TARGET_JERK_FOR_BRAKE, + ) + + elif mode == "brake": + print( + f"===== Drive to {self.TARGET_VELOCITY} m/s and brake with {target_acceleration} =====" + ) + lib.command.change_gear(self, "drive") + lib.cui.ready_check("Ready to drive?") + lib.cui.countdown(COUNTDOWN_TIME) + lib.command.accelerate( + self, + self.TARGET_ACCELERATION_FOR_DRIVE, + self.TARGET_VELOCITY, + "drive", + self.TARGET_JERK_FOR_DRIVE, + ) + print("===== Record rosbag =====") + filename = self.get_rosbag_name(mode, target_acceleration) + process = lib.rosbag.record_ros2_bag(filename, lib.rosbag.TOPIC_LIST) + print(f"record rosbag: {filename}") + lib.command.accelerate(self, target_acceleration, 1e-3, "brake") + print("===== End rosbag record =====") + process.terminate() + + else: + print(f"Invalid mode: {mode}") + sys.exit(1) + + process.wait() + print("===== Validate rosbag =====") + is_rosbag_valid = lib.rosbag.validate(filename, self.TOPIC_LIST_FOR_VALIDATION) + if not is_rosbag_valid: + print(f"Rosbag validation error: {filename}") + sys.exit(1) + + is_finished = lib.cui.finish_check(f"Will you continue to check {mode} map?") + + print(f"===== Successfully {mode} map checking finished! =====") + + def get_min_max_acceleration(self, mode): + if mode == "accel": + return 0.0, self.MAX_ACCEL + if mode == "brake": + return self.MIN_ACCEL, 0.0 + if mode == "sub_brake": + return self.MIN_ACCEL_SUB_BRAKE, 0.0 + + def get_rosbag_name(self, mode, target_acceleration): + current_time = datetime.now().strftime("%Y%m%d-%H%M%S") + filename = "_".join( + [ + "constant_acceleration_cmd", + mode, + str(target_acceleration), + current_time, + ] + ) + return filename + + +def main(args=None): + rclpy.init(args=args) + + tester = DataCollectingAccelerationCmd() + tester.run() + + tester.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py b/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py new file mode 100755 index 00000000..6b968dbc --- /dev/null +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py @@ -0,0 +1,241 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from datetime import datetime +import os +import sys + +from ament_index_python.packages import get_package_share_directory +from autoware_control_msgs.msg import Control +from autoware_vehicle_msgs.msg import ControlModeReport +from autoware_vehicle_msgs.msg import GearCommand +from autoware_vehicle_msgs.msg import GearReport +from autoware_vehicle_msgs.msg import VelocityReport +from autoware_vehicle_msgs.srv import ControlModeCommand +import lib.command +import lib.cui +import lib.rosbag +import lib.system +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32 +from tier4_vehicle_msgs.msg import ActuationCommandStamped +import yaml + +COUNTDOWN_TIME = 3 # [sec] + + +class DataCollectingActuationCmd(Node): + def __init__(self): + super().__init__("data_collecting_actuation_cmd") + + package_share_directory = get_package_share_directory("control_data_collecting_tool") + topic_file_path = os.path.join( + package_share_directory, "config/constant_cmd_param", "actuation_cmd_param.yaml" + ) + with open(topic_file_path, "r") as file: + topic_data = yaml.safe_load(file) + + self.TARGET_VELOCITY = topic_data["/data_collecting_actuation_cmd"]["ros__parameters"][ + "TARGET_VELOCITY" + ] + self.TARGET_ACTUATION_FOR_ACCEL = topic_data["/data_collecting_actuation_cmd"][ + "ros__parameters" + ]["TARGET_ACTUATION_FOR_ACCEL"] + self.TARGET_ACTUATION_FOR_BRAKE = topic_data["/data_collecting_actuation_cmd"][ + "ros__parameters" + ]["TARGET_ACTUATION_FOR_BRAKE"] + self.MAX_ACCEL_PEDAL = topic_data["/data_collecting_actuation_cmd"]["ros__parameters"][ + "MAX_ACCEL_PEDAL" + ] + self.MIN_BRAKE_PEDAL = topic_data["/data_collecting_actuation_cmd"]["ros__parameters"][ + "MIN_BRAKE_PEDAL" + ] + self.TOPIC_LIST_FOR_VALIDATION = topic_data["/data_collecting_actuation_cmd"][ + "ros__parameters" + ]["topics"] + self.NODE_LIST_FOR_VALIDATION = topic_data["/data_collecting_actuation_cmd"][ + "ros__parameters" + ]["validation_nodes"] + + self.client_control_mode = self.create_client( + ControlModeCommand, "/control/control_mode_request" + ) + + while not self.client_control_mode.wait_for_service(timeout_sec=1.0): + print("Waiting for the control mode service to become available...") + + self.pub_data_collecting_control_cmd = self.create_publisher( + Float32, "/data_collecting_accel_cmd", 1 + ) + self.pub_control_cmd = self.create_publisher(Control, "/control/command/control_cmd", 1) + self.pub_gear_cmd = self.create_publisher(GearCommand, "/control/command/gear_cmd", 1) + + self.sub_velocity_status = self.create_subscription( + VelocityReport, "/vehicle/status/velocity_status", self.on_velocity_status, 1 + ) + self.sub_control_mode = self.create_subscription( + ControlModeReport, "/vehicle/status/control_mode", self.on_control_mode, 1 + ) + self.sub_gear_status = self.create_subscription( + GearReport, "/vehicle/status/gear_status", self.on_gear_status, 1 + ) + + self.control_cmd_timer = self.create_timer(0.03, self.control_cmd_timer_callback) + self.control_cmd_timer.cancel() + self.target_acceleration = 0.0 + + self.current_velocity = 0.0 + self.current_control_mode = ControlModeReport.MANUAL + self.current_gear = GearReport.NONE + + # For commands reset + self.pub_control_data_pedal_input = self.create_publisher( + Float32, "/data_collecting_pedal_input", 1 + ) + self.pub_actuation_cmd = self.create_publisher( + ActuationCommandStamped, "/control/command/actuation_cmd", 1 + ) + + def on_velocity_status(self, msg): + self.current_velocity = msg.longitudinal_velocity + + def on_control_mode(self, msg): + self.current_control_mode = msg.mode + + def on_gear_status(self, msg): + self.current_gear = msg.report + + def control_cmd_timer_callback(self): + control_cmd_msg = Control() + control_cmd_msg.stamp = self.get_clock().now().to_msg() + control_cmd_msg.longitudinal.acceleration = self.target_acceleration + self.pub_control_cmd.publish(control_cmd_msg) + + def run(self): + print("===== Start data_collecting_actuation_cmd =====") + lib.system.check_service_active("autoware.service") + lib.system.check_node_active(self.NODE_LIST_FOR_VALIDATION) + + print("===== Reset commands =====") + lib.command.reset_commands(self) + + print("===== Start collecting constant accel pedal input data =====") + lib.cui.do_check("Do you want to accel pedal input data?", lambda: self.check("accel")) + + print("===== Start collecting constant brake pedal input data =====") + lib.cui.do_check("Do you want to brake pedal input data?", lambda: self.check("brake")) + + print("===== Successfully finished! =====") + + def check(self, mode): + is_finished = False + while not is_finished: + print("===== Input target accel pedal input =====") + min_actuation, max_actuation = self.get_min_max_acceleration(mode) + target_actuation = lib.cui.input_target_value( + mode + " pedal input", min_actuation, max_actuation, "" + ) + + if mode == "accel": + print("===== Record rosbag =====") + filename = self.get_rosbag_name(mode, target_actuation) + process = lib.rosbag.record_ros2_bag(filename, lib.rosbag.TOPIC_LIST) + lib.cui.countdown(COUNTDOWN_TIME) + print(f"record rosbag: {filename}") + + print( + f"===== Drive to {self.TARGET_VELOCITY} m/s with accel pedal input {target_actuation} =====" + ) + lib.command.change_gear(self, "drive") + lib.cui.ready_check("Ready to drive?") + lib.cui.countdown(COUNTDOWN_TIME) + lib.command.actuate( + self, mode, target_actuation, self.TARGET_VELOCITY, break_time=30.0 + ) + print("===== End rosbag record =====") + process.terminate() + lib.command.actuate(self, "brake", self.TARGET_ACTUATION_FOR_BRAKE, 1e-3) + elif mode == "brake": + print( + f"===== Drive to {self.TARGET_VELOCITY} m/s and brake pedal input with {target_actuation} =====" + ) + lib.command.change_gear(self, "drive") + lib.cui.ready_check("Ready to drive?") + lib.cui.countdown(COUNTDOWN_TIME) + lib.command.actuate( + self, + "accel", + self.TARGET_ACTUATION_FOR_ACCEL, + self.TARGET_VELOCITY, + ) + filename = self.get_rosbag_name(mode, target_actuation) + process = lib.rosbag.record_ros2_bag(filename, lib.rosbag.TOPIC_LIST) + print("===== Record rosbag =====") + print(f"record rosbag: {filename}") + + lib.command.actuate(self, mode, target_actuation, 1e-3, break_time=60.0) + print("===== End rosbag record =====") + process.terminate() + else: + print(f"Invalid mode: {mode}") + sys.exit(1) + + process.wait() + + print("===== Validate rosbag =====") + is_rosbag_valid = lib.rosbag.validate(filename, self.TOPIC_LIST_FOR_VALIDATION) + if not is_rosbag_valid: + print(f"Rosbag validation error: {filename}") + sys.exit(1) + + is_finished = lib.cui.finish_check( + f"Will you continue to collect {mode} pedal input data?" + ) + + print(f"===== Successfully {mode} map checking finished! =====") + + def get_min_max_acceleration(self, mode): + if mode == "accel": + return 0.0, self.MAX_ACCEL_PEDAL + if mode == "brake": + return 0.0, self.MIN_BRAKE_PEDAL + + def get_rosbag_name(self, mode, target_acceleration): + current_time = datetime.now().strftime("%Y%m%d-%H%M%S") + filename = "_".join( + [ + "constant_actuation_cmd", + mode, + str(target_acceleration), + current_time, + ] + ) + return filename + + +def main(args=None): + rclpy.init(args=args) + + tester = DataCollectingActuationCmd() + tester.run() + + tester.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/control_data_collecting_tool/scripts/calibration/lib/command.py b/control_data_collecting_tool/scripts/calibration/lib/command.py new file mode 100644 index 00000000..3c45897f --- /dev/null +++ b/control_data_collecting_tool/scripts/calibration/lib/command.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +import time + +from autoware_control_msgs.msg import Control +from autoware_vehicle_msgs.msg import ControlModeReport +from autoware_vehicle_msgs.msg import GearCommand +from autoware_vehicle_msgs.msg import GearReport +from autoware_vehicle_msgs.srv import ControlModeCommand +import rclpy +from std_msgs.msg import Float32 +from tier4_vehicle_msgs.msg import ActuationCommandStamped + + +def call_control_mode_request(node, mode): + request = ControlModeCommand.Request() + request.mode = mode + + future = node.client_control_mode.call_async(request) + rclpy.spin_until_future_complete(node, future) + + if future.result() is not None: + print(f"Response from control mode service: {future.result()}") + else: + print("Control mode service call failed.") + sys.exit() + + +def change_mode(node, mode): + print(f"Change mode to {mode}") + if mode == "autonomous": + request = ControlModeCommand.Request.AUTONOMOUS + report = ControlModeReport.AUTONOMOUS + elif mode == "autonomous_velocity_only": + request = ControlModeCommand.Request.AUTONOMOUS_VELOCITY_ONLY + report = ControlModeReport.AUTONOMOUS_VELOCITY_ONLY + elif mode == "autonomous_steering_only": + request = ControlModeCommand.Request.AUTONOMOUS_STEER_ONLY + report = ControlModeReport.AUTONOMOUS_STEER_ONLY + elif mode == "manual": + request = ControlModeCommand.Request.MANUAL + report = ControlModeReport.MANUAL + else: + print(f"Invalid mode: {mode}") + sys.exit(1) + + call_control_mode_request(node, request) + + while rclpy.ok(): + rclpy.spin_once(node) + if node.current_control_mode == report: + break + + +def change_gear(node, target_gear): + print(f"Change gear to {target_gear}") + if target_gear == "neutral": + command = GearCommand.NEUTRAL + report = GearReport.NEUTRAL + elif target_gear == "drive": + command = GearCommand.DRIVE + report = GearReport.DRIVE + elif target_gear == "reverse": + command = GearCommand.REVERSE + report = GearReport.REVERSE + else: + print(f"Invalid gear: {target_gear}") + sys.exit(1) + + gear_cmd_msg = GearCommand() + gear_cmd_msg.stamp = node.get_clock().now().to_msg() + gear_cmd_msg.command = command + node.pub_gear_cmd.publish(gear_cmd_msg) + + while rclpy.ok(): + rclpy.spin_once(node) + if node.current_gear == report: + print(f"Current gear is {target_gear}") + break + + +def accelerate( + node, target_acceleration, target_velocity, mode, target_jerk=None, break_time=120.0 +): + print(f"Accelerate with {target_acceleration} m/s^2.") + start_time = time.time() + + if target_jerk is None: + acceleration_cmd = target_acceleration + else: + acceleration_cmd = 0.0 + + def check_condition(acceleration_cmd, target_acceleration, mode): + if mode == "drive": + return acceleration_cmd < target_acceleration - 1e-3 + else: + return acceleration_cmd > target_acceleration + 1e-3 + + while check_condition(acceleration_cmd, target_acceleration, mode): + acceleration_cmd += target_jerk / 10.0 + data_collecting_control_cmd = acceleration_cmd + node.pub_data_collecting_control_cmd.publish( + Float32(data=float(data_collecting_control_cmd)) + ) + time.sleep(0.1) + + data_collecting_control_cmd = target_acceleration + node.pub_data_collecting_control_cmd.publish(Float32(data=float(data_collecting_control_cmd))) + + while rclpy.ok(): + rclpy.spin_once(node) + if (mode == "drive" and node.current_velocity >= target_velocity) or ( + mode == "brake" and node.current_velocity <= target_velocity + ): + print(f"Reached {target_velocity} m/s.") + data_collecting_control_cmd = 0.0 if mode == "drive" else -2.5 + node.pub_data_collecting_control_cmd.publish( + Float32(data=float(data_collecting_control_cmd)) + ) + break + + if time.time() - start_time > break_time: + print("break : " + str(break_time) + " has passed.") + break + + time.sleep(1) + + +def actuate(node, mode, target_command, target_velocity, break_time=120.0): + print(f"Actuate with {mode} pedal input: {target_command}.") + start_time = time.time() + + data_collecting_pedal_input = 0.0 + if mode == "accel": + data_collecting_pedal_input = target_command + elif mode == "brake": + data_collecting_pedal_input = -target_command + else: + print(f"Invalid mode: {mode}") + sys.exit(1) + + node.pub_control_data_pedal_input.publish(Float32(data=float(data_collecting_pedal_input))) + + while rclpy.ok(): + rclpy.spin_once(node) + if (mode == "accel" and node.current_velocity >= target_velocity) or ( + mode == "brake" and node.current_velocity <= target_velocity + ): + print(f"Reached {target_velocity} m/s.") + data_collecting_pedal_input = 0.0 + if mode == "accel": + data_collecting_pedal_input = 0.0 + elif mode == "brake": + data_collecting_pedal_input = -0.8 + node.pub_control_data_pedal_input.publish( + Float32(data=float(data_collecting_pedal_input)) + ) + break + + if time.time() - start_time > break_time: + print("break : " + str(break_time) + " has passed.") + break + + time.sleep(1) + + +def reset_commands(node): + control_cmd_msg = Control() + control_cmd_msg.stamp = node.get_clock().now().to_msg() + control_cmd_msg.longitudinal.acceleration = 0.0 + control_cmd_msg.lateral.steering_tire_angle = 0.0 + node.pub_control_cmd.publish(control_cmd_msg) + print("Reset control command.") + + actuation_cmd_msg = ActuationCommandStamped() + actuation_cmd_msg.header.stamp = node.get_clock().now().to_msg() + actuation_cmd_msg.actuation.accel_cmd = 0.0 + actuation_cmd_msg.actuation.brake_cmd = 0.0 + node.pub_actuation_cmd.publish(actuation_cmd_msg) + print("Reset actuation command.") + + gear_cmd_msg = GearCommand() + gear_cmd_msg.stamp = node.get_clock().now().to_msg() + gear_cmd_msg.command = GearCommand.NEUTRAL + node.pub_gear_cmd.publish(gear_cmd_msg) + print("Reset gear command.") diff --git a/control_data_collecting_tool/scripts/calibration/lib/cui.py b/control_data_collecting_tool/scripts/calibration/lib/cui.py new file mode 100644 index 00000000..19169965 --- /dev/null +++ b/control_data_collecting_tool/scripts/calibration/lib/cui.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +import time + + +def countdown(n): + while n > 0: + print(n) + time.sleep(1) + n -= 1 + + +def input_yes_or_no(description): + while True: + try: + answer = input(f"{description} (yes/no) > ").lower() + + if answer in {"yes", "no"}: + return answer + else: + print("Please enter 'yes' or 'no'.") + except KeyboardInterrupt as e: + print("\nOperation canceled by user : " + str(e)) + sys.exit(1) + except Exception as e: + print(e) + print("Please enter 'yes' or 'no'.") + + +def ready_check(description): + is_ready = False + while not is_ready: + answer = input_yes_or_no(description) + + if answer == "yes": + is_ready = True + + +def do_check(description, func): + answer = input_yes_or_no(description) + + if answer == "yes": + try: + func() + except Exception as e: + print(f"Error occurred while executing the function: {e}") + + +def finish_check(description): + answer = input_yes_or_no(description) + + if answer == "yes": + return False + else: + return True + + +def input_target_value(description, min_value, max_value, unit): + while True: + try: + value = float(input(f"Target {description} [{min_value} ~ {max_value} {unit}] > ")) + if value < min_value or max_value < value: + print(f"Input target {description} is out of range.") + continue + break + except KeyboardInterrupt as e: + print("\nOperation canceled by user : " + str(e)) + sys.exit(1) + except Exception as e: + print(e) + print("Please input number.") + + print(f"Target {description}: {value}") + return value diff --git a/control_data_collecting_tool/scripts/calibration/lib/rosbag.py b/control_data_collecting_tool/scripts/calibration/lib/rosbag.py new file mode 100644 index 00000000..5597528b --- /dev/null +++ b/control_data_collecting_tool/scripts/calibration/lib/rosbag.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import re +import subprocess + +from rclpy.serialization import deserialize_message +import rosbag2_py +from rosidl_runtime_py.utilities import get_message + +TOPIC_LIST = "/control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*)" + + +def record_ros2_bag(bag_name, topic_list): + command = ["ros2", "bag", "record", "-o", bag_name, "-e", topic_list] + + process = subprocess.Popen( + command, stdin=subprocess.PIPE, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL + ) + + return process + + +def validate(bag_name, topic_list): + try: + result = subprocess.run( + ["ros2", "bag", "info", bag_name], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=True, + ) + output = result.stdout + + missing_topics = [] + for topic_name in topic_list: + pattern = rf"Topic: {re.escape(topic_name)}\s+\|.+?\| Count: (\d+)" + match = re.search(pattern, output) + + if match: + count = int(match.group(1)) + if count == 0: + missing_topics.append(topic_name) + else: + missing_topics.append(topic_name) + + if missing_topics: + print("The following topics are missing:") + for topic_name in missing_topics: + print(f"- {topic_name}") + return False + + return True + except subprocess.CalledProcessError as e: + print(f"Error running ros2 bag info: {e.stderr.strip()}") + return False + except Exception as e: + print(f"Unexpected error: {e}") + return False + + +def get_topics(rosbag_path, topic_names): + storage_options = rosbag2_py.StorageOptions(uri=rosbag_path, storage_id="sqlite3") + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + topic_filter = rosbag2_py.StorageFilter(topics=topic_names) + reader.set_filter(topic_filter) + + topic_types = reader.get_all_topics_and_types() + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + topics = {} + for topic_name in topic_names: + topics[topic_name] = [] + + while reader.has_next(): + topic, data, stamp = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + topics[topic].append(msg) + + return topics diff --git a/control_data_collecting_tool/scripts/calibration/lib/system.py b/control_data_collecting_tool/scripts/calibration/lib/system.py new file mode 100644 index 00000000..6388b681 --- /dev/null +++ b/control_data_collecting_tool/scripts/calibration/lib/system.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import subprocess +import sys + + +def get_active_nodes(): + try: + result = subprocess.run( + ["ros2", "node", "list"], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=True, + ) + # Split the output into lines and return as a list + return result.stdout.strip().split("\n") + except subprocess.CalledProcessError as e: + print(f"Error while running ros2 node list: {e.stderr}") + return [] + + +def check_node_active(node_list): + print("Check if necessary nodes are activated.") + active_nodes = get_active_nodes() + is_all_nodes_active = True + + for node in node_list: + if node not in active_nodes: + print(f"Node {node} is inactive.") + is_all_nodes_active = False + + if is_all_nodes_active: + print("All nodes are activated.") + else: + print("Some nodes are inactive.") + sys.exit(1) + + +def check_service_active(service_name): + print(f"Check if the service '{service_name}' is active.") + try: + result = subprocess.run( + ["systemctl", "status", service_name], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=False, + ) + if result.returncode == 4: + print(f"The service '{service_name}' was not found. Proceeding as OK.") + return + + output = result.stdout + if "Active: active (running)" in output: + print(f"The service '{service_name}' is active and running. Exiting script.") + sys.exit(1) + else: + print(f"The service '{service_name}' is NOT active.") + except FileNotFoundError: + print("The systemctl command is not available on this system.") + sys.exit(1) + except Exception as e: + print(f"An error occurred: {e}") + sys.exit(1) diff --git a/control_data_collecting_tool/scripts/courses/along_road.py b/control_data_collecting_tool/scripts/courses/along_road.py index c9374e7d..dff77c68 100644 --- a/control_data_collecting_tool/scripts/courses/along_road.py +++ b/control_data_collecting_tool/scripts/courses/along_road.py @@ -285,7 +285,7 @@ def get_target_velocity( # Calculate sine wave and apply to velocity T = self.sine_period_for_velocity - sine = np.sin(2 * np.pi * current_time / T) * np.sin(np.pi * current_time / T) + sine = np.sin(np.pi * current_time / T) if current_vel > self.target_vel_on_straight_line: target_vel = self.target_vel_on_straight_line + sine @@ -307,7 +307,7 @@ def get_target_velocity( ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) elif self.deceleration_rate <= achievement_rate: target_vel = max( - current_vel - self.params.a_max / acc_kp_of_pure_pursuit * (1.0 + 0.5 * sine), + current_vel - self.params.a_max / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine), self.velocity_on_curve, ) diff --git a/control_data_collecting_tool/scripts/courses/base_course.py b/control_data_collecting_tool/scripts/courses/base_course.py index 5d4ccf92..8246c76b 100644 --- a/control_data_collecting_tool/scripts/courses/base_course.py +++ b/control_data_collecting_tool/scripts/courses/base_course.py @@ -70,6 +70,32 @@ def choose_target_velocity_acc(self, collected_data_counts_of_vel_acc, mask_vel_ return min_index_list[np.random.randint(0, len(min_index_list))] + def choose_target_velocity_and_actuation_cmd( + self, + collected_data_counts_of_vel_pedal_input, + ): + min_num_data = 1e12 + min_data_num_margin = 20 + min_index_list = [] + + for i in range(self.params.collecting_data_min_n_v, self.params.collecting_data_max_n_v): + for j in range(len(collected_data_counts_of_vel_pedal_input[0])): + if ( + min_num_data - min_data_num_margin + > collected_data_counts_of_vel_pedal_input[i, j] + ): + min_num_data = collected_data_counts_of_vel_pedal_input[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_pedal_input[i, j] + ): + min_index_list.append((j, i)) + + return min_index_list[np.random.randint(0, len(min_index_list))] + def get_target_velocity( self, nearestIndex, @@ -81,7 +107,17 @@ def get_target_velocity( mask_vel_acc, mask_vel_steer, ): - pass + return None + + def get_target_pedal_input( + self, + nearestIndex, + current_time, + current_vel, + collected_data_counts_of_vel_accel_pedal_input, + collected_data_counts_of_vel_brake_pedal_input, + ): + return None def set_vertices(self, A, B, C, D): self.A = A diff --git a/control_data_collecting_tool/scripts/courses/figure_eight.py b/control_data_collecting_tool/scripts/courses/figure_eight.py index 71e4e2fe..535ff471 100644 --- a/control_data_collecting_tool/scripts/courses/figure_eight.py +++ b/control_data_collecting_tool/scripts/courses/figure_eight.py @@ -211,8 +211,7 @@ def get_target_velocity( self.previous_part = part # Calculate sine wave and apply to velocity - T = self.sine_period_for_velocity - sine = np.sin(2 * np.pi * current_time / T) * np.sin(np.pi * current_time / T) + sine = np.sin(np.pi * current_time / self.sine_period_for_velocity) if current_vel > self.target_vel_on_straight_line: target_vel = self.target_vel_on_straight_line + sine + 1.5 * sine - 1.0 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 108df22d..29594441 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", - 35.0, + 15.0, ParameterDescriptor( description="The distance referenced ahead of the vehicle for collecting steering angle data" ), @@ -774,6 +774,7 @@ def __init__(self, step: float, param_dict): ] # Circle radius for trajectory generation. self.enclosing_R = param_dict["enclosing_radius"] self.vel_hist = deque([float(0.0)] * 30, maxlen=30) # Velocity history for smoothing. + self.pedal_hist = deque([float(0.0)] * 30, maxlen=30) self.previous_updated_time = 0.0 # Timestamp for tracking updates. # Initialize velocity and acceleration targets for trajectory segmentation. @@ -784,6 +785,13 @@ def __init__(self, step: float, param_dict): # Set the initial vehicle phase and other state variables. self.vehicle_phase = "acceleration" # Vehicle's current motion phase. self.const_velocity_start_time = 0.0 # Start time for constant velocity phase. + self.acceleration_start_time = 0.0 + self.deceleration_start_time = 0.0 + self.target_accel_pedal_input_on_segmentation = 0.0 + self.target_brake_pedal_input_on_segmentation = 0.0 + self.sine_period_for_velocity = 10.0 + self.sine_period_for_pedal_input = 30.0 + self.max_phase_time = 30.0 self.updated_target_velocity = ( False # Indicates whether the target velocity has been updated. ) @@ -1024,87 +1032,74 @@ def get_target_velocity( ) 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] - self.acc_on_line = self.target_acc_on_segmentation # Set the vehicle's phase to "acceleration" 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 - # 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 + T = self.sine_period_for_velocity + sine = np.sin(2 * np.pi * current_time / T) - target_acc = 0.0 # Handle acceleration phase if self.vehicle_phase == "acceleration": - if current_vel < self.target_vel_on_segmentation - 1.0 * abs( + if current_vel < self.target_vel_on_segmentation - 2.0 * abs( self.target_acc_on_segmentation ): # Increase velocity with a maximum allowable acceleration - target_acc = ( - self.alpha * self.params.a_max / acc_kp_of_pure_pursuit * (0.4 + 0.6 * sine) + target_vel = current_vel + self.params.a_max / acc_kp_of_pure_pursuit * ( + 1.25 + 0.50 * sine ) else: # Increase velocity with a absolute target acceleration - target_acc = ( - abs(self.target_acc_on_segmentation) / acc_kp_of_pure_pursuit + 0.1 * sine - ) + target_vel = current_vel + abs( + self.target_acc_on_segmentation + ) / acc_kp_of_pure_pursuit * (1.25 + 0.50 * 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 * sine - 1.0 + + # 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( + if current_vel < self.target_vel_on_segmentation - 2.0 * abs( self.target_acc_on_segmentation ): # Decrease velocity with a maximum deceleration - target_acc = ( - -self.alpha * self.params.a_max / acc_kp_of_pure_pursuit * (0.4 + 0.6 * sine) + target_vel = current_vel - self.params.a_max / acc_kp_of_pure_pursuit * ( + 1.25 + 0.50 * sine ) else: # Decrease velocity with a absolute target acceleration - target_acc = ( - -abs(self.target_acc_on_segmentation) / acc_kp_of_pure_pursuit + 0.1 * sine - ) + target_vel = current_vel - abs( + self.target_acc_on_segmentation + ) / acc_kp_of_pure_pursuit * (1.25 + 0.50 * sine) # Reset velocity update flag when deceleration is complete - if current_vel < self.target_vel_on_segmentation / 4.0: + if ( + current_vel + < self.target_vel_on_segmentation + - 1.0 * abs(self.target_acc_on_segmentation) / acc_kp_of_pure_pursuit + ): 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 = 2.0 + 2.0 * sine - # Adjust velocity based on trajectory curvature and lateral acceleration constraints - 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 + 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 ): 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 @@ -1147,10 +1142,83 @@ 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_ + 1.0 * sine**2, max_vel_from_lateral_acc_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]) return target_vel + def get_target_pedal_input( + self, + nearestIndex, + current_time, + current_vel, + collected_data_counts_of_vel_accel_pedal_input, + collected_data_counts_of_vel_brake_pedal_input, + ): + max_velocity = self.params.collecting_data_max_v + if ( + (self.trajectory_list[2].in_direction is not self.trajectory_list[2].out_direction) + or (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) + ): + max_velocity = self.params.collecting_data_max_v / 2 + + # Initialize target acceleration and velocity if not already updated + if not self.updated_target_velocity: + # Choose velocity and acceleration bins based on collected data + ( + self.accel_pedal_input_idx, + self.vel_idx, + ) = self.choose_target_velocity_and_actuation_cmd( + collected_data_counts_of_vel_accel_pedal_input + ) + ( + self.brake_pedal_input_idx, + self.vel_idx, + ) = self.choose_target_velocity_and_actuation_cmd( + collected_data_counts_of_vel_brake_pedal_input + ) + self.target_accel_pedal_input_on_segmentation = ( + self.params.accel_pedal_input_bin_centers[self.accel_pedal_input_idx] + ) + self.target_brake_pedal_input_on_segmentation = ( + self.params.brake_pedal_input_bin_centers[self.brake_pedal_input_idx] + ) + # Set the vehicle's phase to "acceleration" + self.vehicle_phase = "acceleration" + self.updated_target_velocity = True + self.acceleration_start_time = current_time + + T = self.sine_period_for_pedal_input + sine = np.sin(2 * np.pi * current_time / T) + + # Handle acceleration phase + if self.vehicle_phase == "acceleration": + target_pedal_input = self.target_accel_pedal_input_on_segmentation + 0.05 * sine + if current_time - self.acceleration_start_time > self.max_phase_time: + target_pedal_input = ( + self.params.accel_pedal_input_max / 2 * sine + + self.params.accel_pedal_input_max / 2 + ) + if current_vel > max_velocity: + self.vehicle_phase = "deceleration" + self.deceleration_start_time = current_time + + # Handle deceleration phase + if self.vehicle_phase == "deceleration": + target_pedal_input = -self.target_brake_pedal_input_on_segmentation - 0.05 * sine + if current_time - self.deceleration_start_time > self.max_phase_time: + target_pedal_input = ( + -self.params.brake_pedal_input_max / 2 * sine + - self.params.brake_pedal_input_max / 2 + ) + if current_vel < 0.05: + self.updated_target_velocity = False + + return target_pedal_input + def update_trajectory_points( self, nearestIndex, @@ -1193,7 +1261,12 @@ def update_trajectory_points( while len(self.trajectory_length_list) < 4: self.add_trajectory_nearly_straight() self.add_trajectory_nearly_straight() - self.add_trajectory_for_turning(steer_with_minimum_num_of_data) + + if self.params.control_mode == "accel_input": + self.add_trajectory_for_turning(steer_with_minimum_num_of_data) + elif self.params.control_mode == "actuation_cmd": + steer_of_trajectory = 1e-9 # Minimal steer value for turning initialization. + self.add_trajectory_for_turning(steer_of_trajectory) # Concatenate trajectory data from the trajectory list self.trajectory_points = np.concatenate( 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 74270236..74550559 100644 --- a/control_data_collecting_tool/scripts/courses/straight_line_negative.py +++ b/control_data_collecting_tool/scripts/courses/straight_line_negative.py @@ -57,7 +57,7 @@ def get_trajectory_points( t_array = np.arange(start=0.0, stop=total_distance, step=self.step).astype("float") self.yaw = np.zeros(len(t_array)) - self.parts = ["linear" for _ in range(len(t_array.copy()))] + self.parts = ["straight" 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)) @@ -98,10 +98,10 @@ def get_target_velocity( # Calculate sine wave and apply to velocity T = self.sine_period_for_velocity - sine = np.sin(2 * np.pi * current_time / T) * np.sin(np.pi * current_time / T) + sine = np.sin(2 * np.pi * current_time / T) if current_vel > self.target_vel_on_straight_line: - target_vel = self.target_vel_on_straight_line + sine - 1.0 + target_vel = self.target_vel_on_straight_line + sine - 0.5 target_vel = max(target_vel, 0.05) elif current_vel < self.target_vel_on_straight_line - 2.0 * abs( self.target_acc_on_straight_line @@ -120,11 +120,7 @@ def get_target_velocity( self.target_acc_on_straight_line ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) elif self.deceleration_rate <= achievement_rate: - target_vel = ( - (current_vel - self.params.a_max / acc_kp_of_pure_pursuit) - * (1.0 + 0.5 * sine) - * (1.0 - achievement_rate) - ) + target_vel = 0.0 return target_vel 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 c07fda1a..e31e15c0 100644 --- a/control_data_collecting_tool/scripts/courses/straight_line_positive.py +++ b/control_data_collecting_tool/scripts/courses/straight_line_positive.py @@ -98,10 +98,10 @@ def get_target_velocity( # Calculate sine wave and apply to velocity T = self.sine_period_for_velocity - sine = np.sin(2 * np.pi * current_time / T) * np.sin(np.pi * current_time / T) + sine = np.sin(2 * np.pi * current_time / T) if current_vel > self.target_vel_on_straight_line: - target_vel = self.target_vel_on_straight_line + sine - 1.0 + target_vel = self.target_vel_on_straight_line + sine - 0.5 target_vel = max(target_vel, 0.05) elif current_vel < self.target_vel_on_straight_line - 2.0 * abs( self.target_acc_on_straight_line @@ -120,11 +120,7 @@ def get_target_velocity( self.target_acc_on_straight_line ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) elif self.deceleration_rate <= achievement_rate: - target_vel = ( - (current_vel - self.params.a_max / acc_kp_of_pure_pursuit) - * (1.0 + 0.5 * sine) - * (1.0 - achievement_rate) - ) + target_vel = 0.0 return target_vel 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 ec6c1f48..72f61211 100644 --- a/control_data_collecting_tool/scripts/data_collecting_base_node.py +++ b/control_data_collecting_tool/scripts/data_collecting_base_node.py @@ -24,6 +24,7 @@ import numpy as np from rcl_interfaces.msg import ParameterDescriptor from rclpy.node import Node +from tier4_vehicle_msgs.msg import ActuationCommandStamped class DataCollectingBaseNode(Node): @@ -31,6 +32,16 @@ def __init__(self, node_name): super().__init__(node_name) # common params + self.declare_parameter( + "CONTROL_MODE", + "acceleration_cmd", + ParameterDescriptor( + description="Control mode [`acceleration_cmd`, `actuation_cmd`, `external_acceleration_cmd`, `external_actuation_cmd`]" + ), + ) + # set control mode + self.CONTROL_MODE = self.get_parameter("CONTROL_MODE").value + self.declare_parameter( "COURSE_NAME", "eight_course", @@ -145,6 +156,30 @@ def __init__(self, node_name): ParameterDescriptor(description="Maximum jerk in heatmap [m/s^3]"), ) + self.declare_parameter( + "ACCEL_PEDAL_INPUT_MAX", + 0.40, + ) + + self.declare_parameter( + "ACCEL_PEDAL_INPUT_MIN", + 0.00, + ) + + self.declare_parameter("NUM_BINS_ACCEL_PEDAL_INPUT", 4) + + self.declare_parameter( + "BRAKE_PEDAL_INPUT_MAX", + 0.40, + ) + + self.declare_parameter( + "BRAKE_PEDAL_INPUT_MIN", + 0.0, + ) + + self.declare_parameter("NUM_BINS_BRAKE_PEDAL_INPUT", 8) + self.ego_point = np.array([0.0, 0.0]) self.goal_point = np.array([0.0, 0.0]) @@ -176,10 +211,18 @@ def __init__(self, node_name): 10, ) + self.actuate_cmd_subscription_ = self.create_subscription( + ActuationCommandStamped, + "/control/command/actuation_cmd", + self.subscribe_actuate_cmd, + 10, + ) + self._present_kinematic_state = Odometry() self._present_acceleration = AccelWithCovarianceStamped() self.present_operation_mode_ = None self._present_control_mode_ = None + self._present_actuation_cmd = None """ velocity and acceleration grid @@ -274,6 +317,50 @@ def __init__(self, node_name): mask_velocity_abs_steer_rate_path, self.num_bins_v, self.num_bins_abs_steer_rate ) + self.accel_pedal_input_min = ( + self.get_parameter("ACCEL_PEDAL_INPUT_MIN").get_parameter_value().double_value + ) + self.accel_pedal_input_max = ( + self.get_parameter("ACCEL_PEDAL_INPUT_MAX").get_parameter_value().double_value + ) + + self.num_bins_accel_pedal_input = ( + self.get_parameter("NUM_BINS_ACCEL_PEDAL_INPUT").get_parameter_value().integer_value + ) + self.accel_pedal_input_bins = np.linspace( + self.accel_pedal_input_min, + self.accel_pedal_input_max, + self.num_bins_accel_pedal_input + 1, + ) + self.accel_pedal_input_bin_centers = ( + self.accel_pedal_input_bins[:-1] + self.accel_pedal_input_bins[1:] + ) / 2 + self.collected_data_counts_of_vel_accel_pedal_input = np.zeros( + (self.num_bins_v, self.num_bins_accel_pedal_input), dtype=np.int32 + ) + + self.brake_pedal_input_min = ( + self.get_parameter("BRAKE_PEDAL_INPUT_MIN").get_parameter_value().double_value + ) + self.brake_pedal_input_max = ( + self.get_parameter("BRAKE_PEDAL_INPUT_MAX").get_parameter_value().double_value + ) + + self.num_bins_brake_pedal_input = ( + self.get_parameter("NUM_BINS_BRAKE_PEDAL_INPUT").get_parameter_value().integer_value + ) + self.brake_pedal_input_bins = np.linspace( + self.brake_pedal_input_min, + self.brake_pedal_input_max, + self.num_bins_brake_pedal_input + 1, + ) + self.brake_pedal_input_bin_centers = ( + self.brake_pedal_input_bins[:-1] + self.brake_pedal_input_bins[1:] + ) / 2 + self.collected_data_counts_of_vel_brake_pedal_input = np.zeros( + (self.num_bins_v, self.num_bins_brake_pedal_input), dtype=np.int32 + ) + def onOdometry(self, msg): self._present_kinematic_state = msg self.ego_point = np.array( @@ -292,6 +379,9 @@ def subscribe_operation_mode(self, msg): def subscribe_control_mode(self, msg): self._present_control_mode_ = msg.mode + def subscribe_actuate_cmd(self, msg): + self._present_actuation_cmd = msg + def load_mask_from_txt(self, file_path, nx, ny): try: mask = np.loadtxt(file_path, 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 adaa5602..57aeb623 100755 --- a/control_data_collecting_tool/scripts/data_collecting_data_counter.py +++ b/control_data_collecting_tool/scripts/data_collecting_data_counter.py @@ -65,7 +65,7 @@ def __init__(self): self.previous_steer = 0.0 self.previous_acc = 0.0 - self.timer_period_callback = 0.033 # 30ms + self.timer_period_callback = 0.033 self.timer_counter = self.create_timer( self.timer_period_callback, self.timer_callback_counter, @@ -86,6 +86,22 @@ def __init__(self): Int32MultiArray, "/control_data_collecting_tools/collected_data_counts_of_vel_jerk", 10 ) + self.collected_data_counts_of_vel_accel_pedal_input_publisher_ = self.create_publisher( + Int32MultiArray, + "/control_data_collecting_tools/collected_data_counts_of_vel_accel_pedal_input", + 10, + ) + + self.collected_data_counts_of_vel_brake_pedal_input_publisher_ = self.create_publisher( + Int32MultiArray, + "/control_data_collecting_tools/collected_data_counts_of_vel_brake_pedal_input", + 10, + ) + + self.collected_data_counts_of_vel_jerk_publisher_ = self.create_publisher( + Int32MultiArray, "/control_data_collecting_tools/collected_data_counts_of_vel_jerk", 10 + ) + self.vel_hist_publisher_ = self.create_publisher( Float32MultiArray, "/control_data_collecting_tools/vel_hist", 10 ) @@ -105,6 +121,20 @@ def __init__(self): self.get_parameter("LOAD_ROSBAG2_FILES").get_parameter_value().bool_value ) + self.declare_parameter( + "STEER_THRESHOLD_FOR_PEDAL_INPUT_COUNT", + 0.2, + ParameterDescriptor( + description="Threshold of steering angle to count pedal input data" + ), + ) + + self.steer_threshold_for_pedal_count = ( + self.get_parameter("STEER_THRESHOLD_FOR_PEDAL_INPUT_COUNT") + .get_parameter_value() + .double_value + ) + if load_rosbag2_files: # candidates referencing the rosbag data rosbag2_dir_list = [d for d in os.listdir("./") if os.path.isdir(os.path.join("./", d))] @@ -239,6 +269,36 @@ def count_observations(self, v, a, steer, steer_rate, jerk): if 0 <= v_bin < self.num_bins_v and 0 <= jerk_bin < self.num_bins_jerk: self.collected_data_counts_of_vel_jerk[v_bin, jerk_bin] += 1 + def count_pedal_input_observation(self, actuation_cmd, current_vel): + if actuation_cmd is not None: + accel_pedal_input = actuation_cmd.actuation.accel_cmd + brake_pedal_input = actuation_cmd.actuation.brake_cmd + accel_pedal_input_bin = ( + np.digitize(accel_pedal_input, self.accel_pedal_input_bin_centers) - 1 + ) + brake_pedal_input_bin = ( + np.digitize(brake_pedal_input, self.brake_pedal_input_bin_centers) - 1 + ) + v_bin = np.digitize(current_vel, self.v_bins) - 1 + + if accel_pedal_input > 1e-3: + if ( + 0 <= v_bin < self.num_bins_v + and 0 <= accel_pedal_input_bin < self.num_bins_accel_pedal_input + ): + self.collected_data_counts_of_vel_accel_pedal_input[ + v_bin, accel_pedal_input_bin + ] += 1 + + if brake_pedal_input > 1e-3: + if ( + 0 <= v_bin < self.num_bins_v + and 0 <= brake_pedal_input_bin < self.num_bins_brake_pedal_input + ): + self.collected_data_counts_of_vel_brake_pedal_input[ + v_bin, brake_pedal_input_bin + ] += 1 + # call back for counting data points def timer_callback_counter(self): if ( @@ -257,6 +317,7 @@ def timer_callback_counter(self): current_acc = self._present_acceleration.accel.accel.linear.x current_steer_rate = (current_steer - self.previous_steer) / 0.033 current_jerk = (current_acc - self.previous_acc) / 0.033 + pedal_input = self._present_actuation_cmd self.previous_steer = current_steer self.previous_acc = current_acc @@ -265,6 +326,8 @@ def timer_callback_counter(self): self.count_observations( current_vel, current_acc, current_steer, current_steer_rate, current_jerk ) + if abs(current_steer) < self.steer_threshold_for_pedal_count: + self.count_pedal_input_observation(pedal_input, current_vel) self.acc_hist.append(float(current_acc)) self.vel_hist.append(float(current_vel)) @@ -292,6 +355,17 @@ def timer_callback_counter(self): self.collected_data_counts_of_vel_jerk, ) + # + publish_Int32MultiArray( + self.collected_data_counts_of_vel_accel_pedal_input_publisher_, + self.collected_data_counts_of_vel_accel_pedal_input, + ) + + publish_Int32MultiArray( + self.collected_data_counts_of_vel_brake_pedal_input_publisher_, + self.collected_data_counts_of_vel_brake_pedal_input, + ) + # publish acc_hist msg = Float32MultiArray() msg.data = list(self.acc_hist) 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_acceleration_cmd.py similarity index 95% rename from control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py rename to control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py index 23c00e05..e0125369 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_acceleration_cmd.py @@ -26,6 +26,7 @@ from rclpy.node import Node from scipy.spatial.transform import Rotation as R from std_msgs.msg import Bool +from std_msgs.msg import Float32 from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray @@ -42,7 +43,18 @@ def getYaw(orientation_xyzw): class DataCollectingPurePursuitTrajectoryFollower(Node): def __init__(self): - super().__init__("data_collecting_pure_pursuit_trajectory_follower") + super().__init__("data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd") + + # common params + self.declare_parameter( + "CONTROL_MODE", + "acceleration_cmd", + ParameterDescriptor( + description="Control mode [`acceleration_cmd`, `actuation_cmd`, `external_acceleration_cmd`, `external_actuation_cmd`]" + ), + ) + # set control mode + self.CONTROL_MODE = self.get_parameter("CONTROL_MODE").value self.declare_parameter( "pure_pursuit_type", @@ -104,7 +116,6 @@ def __init__(self): ), ) - # lim default values are taken from https://github.com/autowarefoundation/autoware.universe/blob/e90d3569bacaf64711072a94511ccdb619a59464/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml self.declare_parameter( "lon_acc_lim", 2.0, @@ -181,6 +192,14 @@ def __init__(self): ) self.sub_trajectory_ + self.sub_acceleration_cmd_ = self.create_subscription( + Float32, + "/data_collecting_accel_cmd", + self.onAccelerationCmd, + 1, + ) + self.sub_acceleration_cmd_ + self.sub_operation_mode_ = self.create_subscription( OperationModeState, "/system/operation_mode/state", @@ -221,6 +240,7 @@ def __init__(self): self._present_kinematic_state = None self._present_trajectory = None self._present_operation_mode = None + self.acceleration_cmd = None self.stop_request = False self._previous_cmd = np.zeros(2) @@ -237,6 +257,9 @@ def onOdometry(self, msg): def onTrajectory(self, msg): self._present_trajectory = msg + def onAccelerationCmd(self, msg): + self.acceleration_cmd = msg.data + def onOperationMode(self, msg): self._present_operation_mode = msg @@ -475,6 +498,13 @@ def control(self): % pure_pursuit_type ) + # overwrite control cmd when external_accel_input mode + if self.CONTROL_MODE == "external_acceleration_cmd": + acceleration_cmd = self.get_parameter("stop_acc").get_parameter_value().double_value + if self.acceleration_cmd is not None: + acceleration_cmd = self.acceleration_cmd + cmd[0] = acceleration_cmd + cmd_without_noise = 1 * cmd tmp_acc_noise = self.acc_noise_list.pop(0) diff --git a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_actuation_cmd.py b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_actuation_cmd.py new file mode 100755 index 00000000..d9bca263 --- /dev/null +++ b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_actuation_cmd.py @@ -0,0 +1,675 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Proxima Technology Inc, TIER IV +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from accel_brake_map import AccelBrakeMapConverter +from autoware_adapi_v1_msgs.msg import OperationModeState +from autoware_control_msgs.msg import Control as AckermannControlCommand +from autoware_planning_msgs.msg import Trajectory +from autoware_vehicle_msgs.msg import GearCommand +from geometry_msgs.msg import Point +from nav_msgs.msg import Odometry +import numpy as np +from rcl_interfaces.msg import ParameterDescriptor +import rclpy +from rclpy.node import Node +from scipy.spatial.transform import Rotation as R +from std_msgs.msg import Bool +from std_msgs.msg import Float32 +from visualization_msgs.msg import Marker +from visualization_msgs.msg import MarkerArray + +debug_matplotlib_plot_flag = False +if debug_matplotlib_plot_flag: + import matplotlib.pyplot as plt + + plt.rcParams["figure.figsize"] = [8, 8] + + +def getYaw(orientation_xyzw): + return R.from_quat(orientation_xyzw.reshape(-1, 4)).as_euler("xyz")[:, 2] + + +class DataCollectingPurePursuitTrajectoryFollowerActuationCmd(Node): + def __init__(self): + super().__init__("data_collecting_pure_pursuit_trajectory_follower_actuation_cmd") + + # common params + self.declare_parameter( + "CONTROL_MODE", + "acceleration_cmd", + ParameterDescriptor( + description="Control mode [`acceleration_cmd`, `actuation_cmd`, `external_acceleration_cmd`, `external_actuation_cmd`]" + ), + ) + # set control mode + self.CONTROL_MODE = self.get_parameter("CONTROL_MODE").value + + self.declare_parameter( + "pure_pursuit_type", + "linearized", + ParameterDescriptor( + description="Pure pursuit type (`naive` or `linearized` steer control law" + ), + ) + + self.declare_parameter( + "wheel_base", + 2.79, # sample_vehicle_launch/sample_vehicle_description/config/vehicle_info.param.yaml + ParameterDescriptor(description="Wheel base [m]"), + ) + + self.declare_parameter( + "acc_kp", + 1.0, + ParameterDescriptor(description="Pure pursuit accel command proportional gain"), + ) + + self.declare_parameter( + "lookahead_time", + 2.0, + ParameterDescriptor(description="Pure pursuit lookahead length coef [s]"), + ) + + self.declare_parameter( + "min_lookahead", + 2.0, + ParameterDescriptor(description="Pure pursuit lookahead length intercept [m]"), + ) + + self.declare_parameter( + "linearized_pure_pursuit_steer_kp_param", + 2.0, + ParameterDescriptor(description="Linearized pure pursuit steering P gain parameter"), + ) + + self.declare_parameter( + "linearized_pure_pursuit_steer_kd_param", + 2.0, + ParameterDescriptor(description="Linearized pure pursuit steering D gain parameter"), + ) + + self.declare_parameter( + "stop_acc", + -2.0, + ParameterDescriptor( + description="Accel command for stopping data collecting driving [m/s^2]" + ), + ) + + self.declare_parameter( + "stop_jerk_lim", + 2.0, + ParameterDescriptor( + description="Jerk limit for stopping data collecting driving [m/s^3]" + ), + ) + + self.declare_parameter( + "lon_acc_lim", + 2.0, + ParameterDescriptor(description="Longitudinal acceleration limit [m/s^2]"), + ) + + self.declare_parameter( + "lon_jerk_lim", + 5.0, + ParameterDescriptor(description="Longitudinal jerk limit [m/s^3]"), + ) + + self.declare_parameter( + "steer_lim", + 1.0, + ParameterDescriptor(description="Steering angle limit [rad]"), + ) + + self.declare_parameter( + "steer_rate_lim", + 1.0, + ParameterDescriptor(description="Steering angle rate limit [rad/s]"), + ) + + self.declare_parameter( + "acc_noise_amp", + 0.01, + ParameterDescriptor(description="Accel cmd additional sine noise amplitude [m/s^2]"), + ) + + self.declare_parameter( + "acc_noise_min_period", + 5.0, + ParameterDescriptor(description="Accel cmd additional sineW noise minimum period [s]"), + ) + + self.declare_parameter( + "acc_noise_max_period", + 20.0, + ParameterDescriptor(description="Accel cmd additional sine noise maximum period [s]"), + ) + + self.declare_parameter( + "steer_noise_amp", + 0.01, + ParameterDescriptor(description="Steer cmd additional sine noise amplitude [rad]"), + ) + + self.declare_parameter( + "steer_noise_min_period", + 5.0, + ParameterDescriptor(description="Steer cmd additional sine noise minimum period [s]"), + ) + + self.declare_parameter( + "steer_noise_max_period", + 20.0, + ParameterDescriptor(description="Steer cmd additional sine noise maximum period [s]"), + ) + + self.declare_parameter( + "accel_brake_map_path", + "", + descriptor=ParameterDescriptor( + description="Path to the accel/brake map directory", dynamic_typing=True + ), + ) + + self.sub_odometry_ = self.create_subscription( + Odometry, + "/localization/kinematic_state", + self.onOdometry, + 1, + ) + self.sub_odometry_ + + self.sub_trajectory_ = self.create_subscription( + Trajectory, + "/data_collecting_trajectory", + self.onTrajectory, + 1, + ) + self.sub_trajectory_ + + self.sub_pedal_input_ = self.create_subscription( + Float32, + "/data_collecting_pedal_input", + self.onPedalInput, + 1, + ) + self.sub_pedal_input_ + + self.sub_operation_mode_ = self.create_subscription( + OperationModeState, + "/system/operation_mode/state", + self.onOperationMode, + 1, + ) + self.sub_operation_mode_ + + self.sub_stop_request_ = self.create_subscription( + Bool, + "/data_collecting_stop_request", + self.onStopRequest, + 1, + ) + self.sub_stop_request_ + + self.control_cmd_pub_ = self.create_publisher( + AckermannControlCommand, + "/external/selected/control_cmd", + 1, + ) + + self.gear_cmd_pub_ = self.create_publisher( + GearCommand, + "/external/selected/gear_cmd", + 1, + ) + + self.data_collecting_lookahead_marker_array_pub_ = self.create_publisher( + MarkerArray, + "/data_collecting_lookahead_marker_array", + 1, + ) + + # load accel/brake map + path_to_accel_map = ( + self.get_parameter("accel_brake_map_path").get_parameter_value().string_value + + "/accel_map.csv" + ) + path_to_brake_map = ( + self.get_parameter("accel_brake_map_path").get_parameter_value().string_value + + "/brake_map.csv" + ) + self.accel_brake_map = AccelBrakeMapConverter(path_to_accel_map, path_to_brake_map) + + self.timer_period_callback = 0.03 # 30ms + self.timer = self.create_timer(self.timer_period_callback, self.timer_callback) + + self._present_kinematic_state = None + self._present_trajectory = None + self._present_operation_mode = None + self.pedal_input = None + self.stop_request = False + self._previous_cmd = np.zeros(2) + + self.acc_noise_list = [] + self.steer_noise_list = [] + self.acc_history = [] + self.steer_history = [] + self.acc_noise_history = [] + self.steer_noise_history = [] + + def onOdometry(self, msg): + self._present_kinematic_state = msg + + def onTrajectory(self, msg): + self._present_trajectory = msg + + def onPedalInput(self, msg): + self.pedal_input = msg.data + + def onOperationMode(self, msg): + self._present_operation_mode = msg + + def onStopRequest(self, msg): + self.stop_request = msg.data + self.get_logger().info("receive " + str(msg)) + + def timer_callback(self): + if (self._present_trajectory is not None) and (self._present_kinematic_state is not None): + self.control() + + def pure_pursuit_steer_control( + self, + pos_xy_obs, + pos_yaw_obs, + longitudinal_vel_obs, + pos_xy_ref_target, + ): + alpha = ( + np.arctan2(pos_xy_ref_target[1] - pos_xy_obs[1], pos_xy_ref_target[0] - pos_xy_obs[0]) + - pos_yaw_obs[0] + ) + # ang_z = 2.0 * longitudinal_vel_ref_nearest * np.sin(alpha) / wheel_base + # steer = np.arctan(ang_z * wheel_base / longitudinal_vel_ref_nearest) + steer = np.arctan(2.0 * np.sin(alpha)) + + return steer + + def linearized_pure_pursuit_steer_control( + self, + pos_xy_obs, + pos_yaw_obs, + longitudinal_vel_obs, + pos_xy_ref_nearest, + pos_yaw_ref_nearest, + ): + + wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value + + # 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 + + linearized_pure_pursuit_steer_kp_param = ( + self.get_parameter("linearized_pure_pursuit_steer_kp_param") + .get_parameter_value() + .double_value + ) + linearized_pure_pursuit_steer_kd_param = ( + self.get_parameter("linearized_pure_pursuit_steer_kd_param") + .get_parameter_value() + .double_value + ) + + # longitudinal_vel_err = longitudinal_vel_obs - longitudinal_vel_ref_nearest + # pure_pursuit_acc_cmd = -acc_kp * longitudinal_vel_err + + cos_yaw = np.cos(pos_yaw_ref_nearest) + sin_yaw = np.sin(pos_yaw_ref_nearest) + diff_position = pos_xy_obs - pos_xy_ref_nearest + lat_err = -sin_yaw * diff_position[0] + cos_yaw * diff_position[1] + yaw_err = pos_yaw_obs - pos_yaw_ref_nearest + lat_err = np.array([lat_err]).flatten()[0] + yaw_err = np.array([yaw_err]).flatten()[0] + while True: + if yaw_err > np.pi: + yaw_err -= 2.0 * np.pi + if yaw_err < (-np.pi): + yaw_err += 2.0 * np.pi + if np.abs(yaw_err) < np.pi: + break + + lookahead = lookahead_intercept + lookahead_coef * np.abs(longitudinal_vel_obs) + linearized_pure_pursuit_steer_kp = ( + linearized_pure_pursuit_steer_kp_param * wheel_base / (lookahead * lookahead) + ) + linearized_pure_pursuit_steer_kd = ( + linearized_pure_pursuit_steer_kd_param * wheel_base / lookahead + ) + pure_pursuit_steer_cmd = ( + -linearized_pure_pursuit_steer_kp * lat_err - linearized_pure_pursuit_steer_kd * yaw_err + ) + return pure_pursuit_steer_cmd # np.array([pure_pursuit_acc_cmd, pure_pursuit_steer_cmd]) + + def control(self): + # [0] receive topic + present_position = np.array( + [ + self._present_kinematic_state.pose.pose.position.x, + self._present_kinematic_state.pose.pose.position.y, + self._present_kinematic_state.pose.pose.position.z, + ] + ) + present_orientation = 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, + ] + ) + present_linear_velocity = np.array( + [ + self._present_kinematic_state.twist.twist.linear.x, + self._present_kinematic_state.twist.twist.linear.y, + self._present_kinematic_state.twist.twist.linear.z, + ] + ) + present_yaw = getYaw(present_orientation) + + trajectory_position = [] + trajectory_orientation = [] + trajectory_longitudinal_velocity = [] + points = self._present_trajectory.points + for i in range(len(points)): + trajectory_position.append( + [points[i].pose.position.x, points[i].pose.position.y, points[i].pose.position.z] + ) + trajectory_orientation.append( + [ + points[i].pose.orientation.x, + points[i].pose.orientation.y, + points[i].pose.orientation.z, + points[i].pose.orientation.w, + ] + ) + trajectory_longitudinal_velocity.append(points[i].longitudinal_velocity_mps) + trajectory_position = np.array(trajectory_position) + trajectory_orientation = np.array(trajectory_orientation) + trajectory_longitudinal_velocity = np.array(trajectory_longitudinal_velocity) + + is_applying_control = False + if self._present_operation_mode is not None: + if ( + self._present_operation_mode.mode == 3 + and self._present_operation_mode.is_autoware_control_enabled + ): + is_applying_control = True + + nearestIndex = ( + ((trajectory_position[:, :2] - present_position[:2]) ** 2).sum(axis=1).argmin() + ) + + # prepare noise + if len(self.acc_noise_list) == 0: + tmp_noise_amp = ( + np.random.rand() + * self.get_parameter("acc_noise_amp").get_parameter_value().double_value + ) + noise_min_period = ( + self.get_parameter("acc_noise_min_period").get_parameter_value().double_value + ) + noise_max_period = ( + self.get_parameter("acc_noise_max_period").get_parameter_value().double_value + ) + tmp_noise_period = noise_min_period + np.random.rand() * ( + noise_max_period - noise_min_period + ) + dt = self.timer_period_callback + noise_data_num = max(4, int(tmp_noise_period / dt)) + for i in range(noise_data_num): + self.acc_noise_list.append(tmp_noise_amp * np.sin(2.0 * np.pi * i / noise_data_num)) + if len(self.steer_noise_list) == 0: + tmp_noise_amp = ( + np.random.rand() + * self.get_parameter("steer_noise_amp").get_parameter_value().double_value + ) + noise_min_period = ( + self.get_parameter("steer_noise_min_period").get_parameter_value().double_value + ) + noise_max_period = ( + self.get_parameter("steer_noise_max_period").get_parameter_value().double_value + ) + tmp_noise_period = noise_min_period + np.random.rand() * ( + noise_max_period - noise_min_period + ) + + dt = self.timer_period_callback + noise_data_num = max(4, int(tmp_noise_period / dt)) + for i in range(noise_data_num): + self.steer_noise_list.append( + tmp_noise_amp * np.sin(2.0 * np.pi * i / noise_data_num) + ) + + # [1] compute control + targetIndex = 1 * nearestIndex + lookahead_coef = self.get_parameter("lookahead_time").get_parameter_value().double_value + lookahead_intercept = self.get_parameter("min_lookahead").get_parameter_value().double_value + pure_pursuit_lookahead_length = ( + lookahead_coef * present_linear_velocity[0] + lookahead_intercept + ) + + while True: + tmp_distance = np.sqrt( + ((trajectory_position[targetIndex][:2] - present_position[:2]) ** 2).sum() + ) + if tmp_distance > pure_pursuit_lookahead_length: + break + if targetIndex == (len(trajectory_position) - 1): + break + targetIndex += 1 + + pure_pursuit_type = ( + self.get_parameter("pure_pursuit_type").get_parameter_value().string_value + ) + + steer_cmd_without_limit = 0.0 + if pure_pursuit_type == "naive": + steer_cmd_without_limit = self.pure_pursuit_steer_control( + present_position[:2], + present_yaw, + present_linear_velocity[0], + trajectory_position[targetIndex][:2], + ) + elif pure_pursuit_type == "linearized": + steer_cmd_without_limit = self.linearized_pure_pursuit_steer_control( + present_position[:2], + present_yaw, + present_linear_velocity[0], + trajectory_position[targetIndex][:2], + getYaw(trajectory_orientation[targetIndex]), + ) + else: + self.get_logger().info( + 'pure_pursuit_type should be "naive" or "linearized" but is set to "%s" ' + % pure_pursuit_type + ) + + acceleration_cmd = self.get_parameter("stop_acc").get_parameter_value().double_value + if self.CONTROL_MODE == "actuation_cmd" or self.CONTROL_MODE == "external_actuation_cmd": + if self.pedal_input is not None: + acceleration_cmd = self.accel_brake_map.convert_actuation_cmd_to_accel_input( + self.pedal_input, present_linear_velocity[0] + ) + + cmd = np.zeros(2) + cmd[0] = acceleration_cmd + cmd[1] = steer_cmd_without_limit + cmd_without_noise = 1 * cmd + + tmp_acc_noise = self.acc_noise_list.pop(0) + tmp_steer_noise = self.steer_noise_list.pop(0) + + cmd[0] += tmp_acc_noise + cmd[1] += tmp_steer_noise + + # overwrite control_cmd if received stop request + if not self.stop_request: + pass + else: + stop_acc = self.get_parameter("stop_acc").get_parameter_value().double_value + if stop_acc > 0.0: + self.get_logger().info("stop_acc should be negative! Force set to -1.0") + stop_acc = -1.0 + stop_jerk_lim = self.get_parameter("stop_jerk_lim").get_parameter_value().double_value + cmd[0] = max( + stop_acc, self._previous_cmd[0] - stop_jerk_lim * self.timer_period_callback + ) + cmd[1] = 1 * self._previous_cmd[1] + + # apply control_cmd limit + lon_acc_lim = self.get_parameter("lon_acc_lim").get_parameter_value().double_value + steer_lim = self.get_parameter("steer_lim").get_parameter_value().double_value + cmd[0] = np.clip(cmd[0], -lon_acc_lim, lon_acc_lim) + cmd[1] = np.clip(cmd[1], -steer_lim, steer_lim) + acc_diff_limit = ( + self.get_parameter("lon_jerk_lim").get_parameter_value().double_value + * self.timer_period_callback + ) + steer_diff_limit = ( + self.get_parameter("steer_rate_lim").get_parameter_value().double_value + * self.timer_period_callback + ) + cmd[0] = np.clip( + cmd[0], self._previous_cmd[0] - acc_diff_limit, self._previous_cmd[0] + acc_diff_limit + ) + cmd[1] = np.clip( + cmd[1], + self._previous_cmd[1] - steer_diff_limit, + self._previous_cmd[1] + steer_diff_limit, + ) + + if is_applying_control: + self._previous_cmd = cmd.copy() + else: + self._previous_cmd = np.zeros(2) + + # [2] publish actuation cmd + # should be modified + control_cmd_msg = AckermannControlCommand() + control_cmd_msg.stamp = control_cmd_msg.lateral.stamp = ( + control_cmd_msg.longitudinal.stamp + ) = (self.get_clock().now().to_msg()) + control_cmd_msg.longitudinal.velocity = trajectory_longitudinal_velocity[nearestIndex] + control_cmd_msg.longitudinal.acceleration = cmd[0] + control_cmd_msg.lateral.steering_tire_angle = cmd[1] + self.control_cmd_pub_.publish(control_cmd_msg) + + gear_cmd_msg = GearCommand() + gear_cmd_msg.stamp = self.get_clock().now().to_msg() + gear_cmd_msg.command = GearCommand.DRIVE + self.gear_cmd_pub_.publish(gear_cmd_msg) + + # [3] publish marker + marker_array = MarkerArray() + + marker_trajectory = Marker() + marker_trajectory.type = 4 + marker_trajectory.id = 1 + marker_trajectory.header.frame_id = "map" + + marker_trajectory.action = marker_trajectory.ADD + + marker_trajectory.scale.x = 0.6 + marker_trajectory.scale.y = 0.0 + marker_trajectory.scale.z = 0.0 + + marker_trajectory.color.a = 1.0 + marker_trajectory.color.r = 0.0 + marker_trajectory.color.g = 1.0 + marker_trajectory.color.b = 0.0 + + marker_trajectory.lifetime.nanosec = 500000000 + marker_trajectory.frame_locked = True + + marker_trajectory.points = [] + tmp_marker_point = Point() + tmp_marker_point.x = present_position[0] + tmp_marker_point.y = present_position[1] + tmp_marker_point.z = 0.0 + marker_trajectory.points.append(tmp_marker_point) + tmp_marker_point = Point() + tmp_marker_point.x = trajectory_position[targetIndex][0] + tmp_marker_point.y = trajectory_position[targetIndex][1] + tmp_marker_point.z = 0.0 + marker_trajectory.points.append(tmp_marker_point) + + marker_array.markers.append(marker_trajectory) + self.data_collecting_lookahead_marker_array_pub_.publish(marker_array) + + # [99] debug plot + if debug_matplotlib_plot_flag: + self.acc_history.append(1 * cmd[0]) + self.steer_history.append(1 * cmd[1]) + self.acc_noise_history.append(tmp_acc_noise) + self.steer_noise_history.append(tmp_steer_noise) + max_plot_len = 666 + if len(self.acc_history) > max_plot_len: + self.acc_history.pop(0) + self.steer_history.pop(0) + self.acc_noise_history.pop(0) + self.steer_noise_history.pop(0) + dt = self.timer_period_callback + timestamp = -dt * np.array(range(len(self.steer_history)))[::-1] + plt.cla() + plt.clf() + plt.subplot(2, 1, 1) + plt.plot(0, cmd[0], "o", label="current_acc") + plt.plot(0, cmd_without_noise[0], "o", label="acc cmd without noise") + plt.plot(timestamp, self.acc_history, "-", label="acc cmd history") + plt.plot(timestamp, self.acc_noise_history, "-", label="acc noise history") + plt.xlim([-20.5, 0.5]) + plt.ylim([-1, 3]) + plt.ylabel("acc [m/ss]") + plt.legend() + plt.subplot(2, 1, 2) + plt.plot(0, cmd[1], "o", label="current_steer") + plt.plot(0, cmd_without_noise[1], "o", label="steer without noise") + plt.plot(timestamp, self.steer_history, "-", label="steer cmd history") + plt.plot(timestamp, self.steer_noise_history, "-", label="steer noise history") + plt.xlim([-20.5, 0.5]) + plt.ylim([-1.25, 1.25]) + plt.xlabel("future timestamp [s]") + plt.ylabel("steer [rad]") + plt.legend() + plt.pause(0.01) + + +def main(args=None): + rclpy.init(args=args) + + data_collecting_pure_pursuit_trajectory_follower_actuation_cmd = ( + DataCollectingPurePursuitTrajectoryFollowerActuationCmd() + ) + + rclpy.spin(data_collecting_pure_pursuit_trajectory_follower_actuation_cmd) + + data_collecting_pure_pursuit_trajectory_follower_actuation_cmd.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() 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 c3c2d41b..9e210083 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -29,6 +29,7 @@ import rclpy from scipy.spatial.transform import Rotation as R from std_msgs.msg import Bool +from std_msgs.msg import Float32 from std_msgs.msg import Int32MultiArray from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray @@ -154,6 +155,12 @@ def __init__(self): ParameterDescriptor(description="Maximum velocity for data collection [m/s^2]"), ) + self.pedal_input_pub_ = self.create_publisher( + Float32, + "/data_collecting_pedal_input", + 1, + ) + self.trajectory_for_collecting_data_pub_ = self.create_publisher( Trajectory, "/data_collecting_trajectory", @@ -244,6 +251,26 @@ def __init__(self): ) self.collected_data_counts_of_vel_steer_subscription_ + self.collected_data_counts_of_vel_accel_pedal_input_subscription_ = ( + self.create_subscription( + Int32MultiArray, + "/control_data_collecting_tools/collected_data_counts_of_vel_accel_pedal_input", + self.subscribe_collected_data_counts_of_vel_accel_pedal_input, + 10, + ) + ) + self.collected_data_counts_of_vel_accel_pedal_input_subscription_ + + self.collected_data_counts_of_vel_brake_pedal_input_subscription_ = ( + self.create_subscription( + Int32MultiArray, + "/control_data_collecting_tools/collected_data_counts_of_vel_brake_pedal_input", + self.subscribe_collected_data_counts_of_vel_brake_pedal_input, + 10, + ) + ) + self.collected_data_counts_of_vel_brake_pedal_input_subscription_ + if debug_matplotlib_plot_flag: self.fig, self.axs = plt.subplots(4, 1, figsize=(12, 20)) plt.ion() @@ -258,6 +285,20 @@ def subscribe_collected_data_counts_of_vel_steer(self, msg): cols = msg.layout.dim[1].size self.collected_data_counts_of_vel_steer = np.array(msg.data).reshape((rows, cols)) + def subscribe_collected_data_counts_of_vel_accel_pedal_input(self, msg): + rows = msg.layout.dim[0].size + cols = msg.layout.dim[1].size + self.collected_data_counts_of_vel_accel_pedal_input = np.array(msg.data).reshape( + (rows, cols) + ) + + def subscribe_collected_data_counts_of_vel_brake_pedal_input(self, msg): + rows = msg.layout.dim[0].size + cols = msg.layout.dim[1].size + self.collected_data_counts_of_vel_brake_pedal_input = np.array(msg.data).reshape( + (rows, cols) + ) + def onDataCollectingArea(self, msg): self._data_collecting_area_polygon = msg self.updateNominalTargetTrajectory() @@ -404,22 +445,6 @@ def callback_trajectory_generator(self): or self.trajectory_position_data is not None or self.trajectory_yaw_data is not None ): - # [0] update nominal target trajectory if changing related ros2 params - target_longitudinal_velocity = ( - self.get_parameter("target_longitudinal_velocity") - .get_parameter_value() - .double_value - ) - - window = self.get_parameter("mov_ave_window").get_parameter_value().integer_value - - if ( - np.abs(target_longitudinal_velocity - self.current_target_longitudinal_velocity) - > 1e-6 - or window != self.current_window - ): - self.updateNominalTargetTrajectory() - # [1] receive observation from topic present_position = np.array( [ @@ -510,21 +535,49 @@ def callback_trajectory_generator(self): ((trajectory_position_data[index_range] - present_position[:2]) ** 2).sum(axis=1) ) index_array_near = np.argsort(distance) - self.nearestIndex = index_range[index_array_near[0]] - # set target velocity + if self.present_operation_mode_ == 3: + self.nearestIndex = index_range[index_array_near[0]] + # set target velocity or target pedal input present_vel = present_linear_velocity[0] present_acc = self._present_acceleration.accel.accel.linear.x current_time = self.get_clock().now().nanoseconds / 1e9 - target_vel = self.course.get_target_velocity( - self.nearestIndex, - current_time, - present_vel, - present_acc, - self.collected_data_counts_of_vel_acc, - self.collected_data_counts_of_vel_steer, - self.mask_vel_acc, - self.mask_vel_steer, - ) + + target_vel = 0.0 + target_pedal_input = 0.0 + if self.CONTROL_MODE == "acceleration_cmd": + target_vel = self.course.get_target_velocity( + self.nearestIndex, + current_time, + present_vel, + present_acc, + self.collected_data_counts_of_vel_acc, + self.collected_data_counts_of_vel_steer, + self.mask_vel_acc, + self.mask_vel_steer, + ) + elif self.CONTROL_MODE == "actuation_cmd": + if self.COURSE_NAME == "reversal_loop_circle": + target_pedal_input = self.course.get_target_pedal_input( + self.nearestIndex, + current_time, + present_vel, + self.collected_data_counts_of_vel_accel_pedal_input, + self.collected_data_counts_of_vel_brake_pedal_input, + ) + else: + msg = Bool() + msg.data = True + self.pub_stop_request_.publish(msg) + self.get_logger().error( + f"Control mode {self.CONTROL_MODE} is not supported when the course is {self.COURSE_NAME}" + ) + elif ( + self.CONTROL_MODE == "external_acceleration_cmd" + or self.CONTROL_MODE == "external_actuation_cmd" + ): + pass # do nothing + else: + self.get_logger().error(f"Invalid control mode : {self.CONTROL_MODE}") trajectory_longitudinal_velocity_data = np.array( [target_vel for _ in range(len(trajectory_position_data))] @@ -611,7 +664,12 @@ def callback_trajectory_generator(self): ) # [6] publish - # [6-1] publish trajectory + + # [6-1] publish pedal input + if self.CONTROL_MODE == "actuation_cmd": + self.pedal_input_pub_.publish(Float32(data=float(target_pedal_input))) + + # [6-2] publish trajectory if self.course.closed: pub_trajectory_index = np.arange( self.nearestIndex, self.nearestIndex + int(50 / self.trajectory_step) @@ -647,10 +705,10 @@ def callback_trajectory_generator(self): self.trajectory_for_collecting_data_pub_.publish(tmp_trajectory) - # [6-2] publish marker_array + # [6-3] publish marker_array marker_array = MarkerArray() - # [6-2a] local trajectory + # [6-3a] local trajectory marker_trajectory_1 = Marker() marker_trajectory_1.type = 4 marker_trajectory_1.id = 1 @@ -717,7 +775,7 @@ def callback_trajectory_generator(self): marker_array.markers.append(boundary_marker_trajectory_1) - # [6-2b] whole trajectory + # [6-3b] whole trajectory marker_trajectory_2 = Marker() marker_trajectory_2.type = 4 marker_trajectory_2.id = 0 @@ -748,7 +806,7 @@ def callback_trajectory_generator(self): marker_array.markers.append(marker_trajectory_2) - # [6-2c] add arrow + # [6-3c] add arrow marker_arrow = Marker() marker_arrow.type = marker_arrow.ARROW marker_arrow.id = 2 @@ -793,14 +851,14 @@ def callback_trajectory_generator(self): self.data_collecting_trajectory_marker_array_pub_.publish(marker_array) - # [6-3] stop request + # [6-4] stop request # self.get_logger().info("self.course.check_in_boundary(present_position): {}".format(self.course.check_in_boundary(present_position))) if not self.course.check_in_boundary(present_position): msg = Bool() msg.data = True self.pub_stop_request_.publish(msg) - # [6-4] update trajectory data if necessary + # [6-5] update trajectory data if necessary ( self.nearestIndex, self.trajectory_position_data, diff --git a/control_data_collecting_tool/scripts/params.py b/control_data_collecting_tool/scripts/params.py index 16c6224e..cf050199 100644 --- a/control_data_collecting_tool/scripts/params.py +++ b/control_data_collecting_tool/scripts/params.py @@ -48,7 +48,7 @@ def __init__(self, param_dict): 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 - collecting_data_min_v, collecting_data_max_v = ( + self.collecting_data_min_v, self.collecting_data_max_v = ( param_dict["COLLECTING_DATA_V_MIN"], param_dict["COLLECTING_DATA_V_MAX"], ) @@ -58,12 +58,40 @@ def __init__(self, param_dict): param_dict["COLLECTING_DATA_A_MAX"], ) - self.collecting_data_min_n_v = max([np.digitize(collecting_data_min_v, self.v_bins) - 1, 0]) + self.collecting_data_min_n_v = max( + [np.digitize(self.collecting_data_min_v, self.v_bins) - 1, 0] + ) self.collecting_data_max_n_v = ( - min([np.digitize(collecting_data_max_v, self.v_bins) - 1, self.num_bins_v - 1]) + 1 + min([np.digitize(self.collecting_data_max_v, self.v_bins) - 1, self.num_bins_v - 1]) + 1 ) - self.collecting_data_min_n_a = max([np.digitize(collecting_data_min_a, self.v_bins) - 1, 0]) + self.collecting_data_min_n_a = max([np.digitize(collecting_data_min_a, self.a_bins) - 1, 0]) self.collecting_data_max_n_a = ( - min([np.digitize(collecting_data_max_a, self.v_bins) - 1, self.num_bins_a - 1]) + 1 + min([np.digitize(collecting_data_max_a, self.a_bins) - 1, self.num_bins_a - 1]) + 1 + ) + + self.accel_pedal_input_min = param_dict["ACCEL_PEDAL_INPUT_MIN"] + self.accel_pedal_input_max = param_dict["ACCEL_PEDAL_INPUT_MAX"] + self.num_bins_accel_pedal_input = param_dict["NUM_BINS_ACCEL_PEDAL_INPUT"] + self.accel_pedal_input_bins = np.linspace( + self.accel_pedal_input_min, + self.accel_pedal_input_max, + self.num_bins_accel_pedal_input + 1, ) + self.accel_pedal_input_bin_centers = ( + self.accel_pedal_input_bins[:-1] + self.accel_pedal_input_bins[1:] + ) / 2 + + self.brake_pedal_input_min = param_dict["BRAKE_PEDAL_INPUT_MIN"] + self.brake_pedal_input_max = param_dict["BRAKE_PEDAL_INPUT_MAX"] + self.num_bins_brake_pedal_input = param_dict["NUM_BINS_BRAKE_PEDAL_INPUT"] + self.brake_pedal_input_bins = np.linspace( + self.brake_pedal_input_min, + self.brake_pedal_input_max, + self.num_bins_brake_pedal_input + 1, + ) + self.brake_pedal_input_bin_centers = ( + self.brake_pedal_input_bins[:-1] + self.brake_pedal_input_bins[1:] + ) / 2 + + self.control_mode = param_dict["CONTROL_MODE"]