From 13efac1eef1d3f8d82d2592fd5e6053dc45cba6c Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Fri, 27 Dec 2024 06:29:34 +0900 Subject: [PATCH 01/17] Add actuation mode Signed-off-by: Yoshihiro Kogure --- control_data_collecting_tool/README.md | 3 +- .../config/common_param.yaml | 15 +- .../control_data_collecting_tool.launch.py | 140 +++- .../scripts/accel_brake_map.py | 126 ++++ .../data_collecting_acceleration_cmd.py | 216 ++++++ .../data_collecting_actuation_cmd.py | 223 ++++++ .../scripts/calibration/lib/command.py | 197 ++++++ .../scripts/calibration/lib/cui.py | 89 +++ .../scripts/calibration/lib/rosbag.py | 100 +++ .../scripts/calibration/lib/system.py | 79 +++ .../scripts/data_collecting_base_node.py | 90 +++ .../scripts/data_collecting_data_counter.py | 60 ++ ...t_trajectory_follower_acceleration_cmd.py} | 33 +- ...rsuit_trajectory_follower_actuation_cmd.py | 669 ++++++++++++++++++ .../scripts/data_collecting_rosbag_record.py | 20 +- .../data_collecting_trajectory_publisher.py | 103 ++- .../scripts/params.py | 38 +- 17 files changed, 2125 insertions(+), 76 deletions(-) create mode 100644 control_data_collecting_tool/scripts/accel_brake_map.py create mode 100755 control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py create mode 100755 control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py create mode 100644 control_data_collecting_tool/scripts/calibration/lib/command.py create mode 100644 control_data_collecting_tool/scripts/calibration/lib/cui.py create mode 100644 control_data_collecting_tool/scripts/calibration/lib/rosbag.py create mode 100644 control_data_collecting_tool/scripts/calibration/lib/system.py rename control_data_collecting_tool/scripts/{data_collecting_pure_pursuit_trajectory_follower.py => data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py} (95%) create mode 100755 control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_actuation_cmd.py diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 6a51a257..4c23a811 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -191,13 +191,14 @@ ROS 2 parameters which are common in all trajectories (`/config/common_param.yam | 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 | | `NUM_BINS_STEER` | `int` | Number of bins of steer in heatmap | 20 | | `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_JERK` | `int` | Number of bins of jerk in heatmap ` | 10 | | `V_MIN` | `double` | Minimum velocity in heatmap [m/s] | 0.0 | | `V_MAX` | `double` | Maximum velocity in heatmap [m/s] | 11.5 | | `STEER_MIN` | `double` | Minimum steer in heatmap [rad] | -0.6 | diff --git a/control_data_collecting_tool/config/common_param.yaml b/control_data_collecting_tool/config/common_param.yaml index 1a578f0b..b0974066 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: 10 + NUM_BINS_BRAKE_PEDAL_INPUT: 12 V_MIN: 0.0 V_MAX: 11.5 STEER_MIN: -0.6 @@ -24,6 +31,10 @@ 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.01 + BRAKE_PEDAL_INPUT_MAX: 0.8 + BRAKE_PEDAL_INPUT_MIN: 0.0 MASK_NAME: default VEL_ACC_THRESHOLD: 40 @@ -50,4 +61,4 @@ lon_acc_lim: 1.5 lon_jerk_lim: 0.5 steer_lim: 0.6 - steer_rate_lim: 0.6 + steer_rate_lim: 0.6 \ No newline at end of file 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..6b747499 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/scripts/accel_brake_map.py b/control_data_collecting_tool/scripts/accel_brake_map.py new file mode 100644 index 00000000..08496d6b --- /dev/null +++ b/control_data_collecting_tool/scripts/accel_brake_map.py @@ -0,0 +1,126 @@ +#!/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): + # + map_df = pd.read_csv(path_to_map, delimiter=",", header=0) + self.map_grid = map_df.to_numpy()[:, 1:] + + # + 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] + + # + 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] + + # + self.map_grid_interpolator = RegularGridInterpolator( + (self.pedal_grid_map, self.velocity_grid_map), self.map_grid + ) + + def pedal_to_accel_input(self, pedal, velocity): + # + 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): + velocity = np.clip(velocity, self.min_vel_of_map, self.max_vel_of_map) + + 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) + + accel_input_map = ( + alpha * self.map_grid[:, vel_idx] + (1 - alpha) * self.map_grid[:, vel_idx + 1] + ) + + 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) + actuation_input = 0.0 + + for i in range(len(accel_input_map) - 1): + if accel_input_map[i] <= accel_input_map[i + 1]: + if accel_input_map[i] <= accel_input and 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]: + sign = -1 + if accel_input_map[i + 1] <= accel_input and 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): + # + 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): + # + 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): + # + accel_input = 0.0 + if actuation_cmd > 0.0: + accel_input = self.accel_map_converter.pedal_to_accel_input( + abs(actuation_cmd), velocity + ) + if actuation_cmd <= 0.0: + 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..67ff8abb --- /dev/null +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py @@ -0,0 +1,216 @@ +#!/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 sys + +from std_msgs.msg import Float32 +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 tier4_vehicle_msgs.msg import ActuationCommandStamped + +COUNTDOWN_TIME = 3 # [sec] +TARGET_VELOCITY = 42.5 # [km/h] +TARGET_ACCELERATION_FOR_DRIVE = 1.5 # [m/s^2] +TARGET_ACCELERATION_FOR_BRAKE = -1.5 # [m/s^2] +TARGET_JERK_FOR_DRIVE = 0.5 # [m/s^3] +TARGET_JERK_FOR_BRAKE = -0.5 # [m/s^3] + +MIN_ACCEL = -6.0 +MAX_ACCEL = 2.0 +MIN_ACCEL_SUB_BRAKE = -2.0 + +TOPIC_LIST_FOR_VALIDATION = [ + "/control/command/control_cmd", + "/vehicle/status/velocity_status", + "/vehicle/status/control_mode", +] + +NODE_LIST_FOR_VALIDATION = [ + "/raw_vehicle_cmd_converter" +] + + +class MapAccuracyTester(Node): + def __init__(self): + super().__init__("map_accuracy_tester") + 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 map accuracy tester =====") + lib.system.check_service_active("autoware.service") + lib.system.check_node_active(NODE_LIST_FOR_VALIDATION) + + print(f"===== Set Acceleration to {TARGET_ACCELERATION_FOR_BRAKE} =====") + lib.command.accelerate( + self, TARGET_ACCELERATION_FOR_BRAKE, 1e-3, "brake", TARGET_JERK_FOR_BRAKE + ) + + print("===== Start checking accel map =====") + lib.cui.do_check("Do you want to check accel map?", lambda: self.check("accel")) + + print("===== Start checking brake map =====") + lib.cui.do_check("Do you want to check brake map?", 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 {TARGET_VELOCITY} km/h 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, TARGET_VELOCITY, "drive",break_time=60.0) + print("===== End rosbag record =====") + process.terminate() + lib.command.accelerate( + self, TARGET_ACCELERATION_FOR_BRAKE, 1e-3, "brake", TARGET_JERK_FOR_BRAKE + ) + + elif mode == "brake": + print( + f"===== Drive to {TARGET_VELOCITY} km/h 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, + TARGET_ACCELERATION_FOR_DRIVE, + TARGET_VELOCITY, + "drive", + 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, TOPIC_LIST_FOR_VALIDATION) + if not is_rosbag_valid: + print(f"Rosag 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, MAX_ACCEL + if mode == "brake": + return MIN_ACCEL, 0.0 + if mode == "sub_brake": + return 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( + [ + "acceleration_accuracy", + mode, + str(target_acceleration), + current_time, + ] + ) + return filename + + +def main(args=None): + rclpy.init(args=args) + + tester = MapAccuracyTester() + 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..8002d939 --- /dev/null +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py @@ -0,0 +1,223 @@ +#!/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 sys + +from std_msgs.msg import Float32 +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 tier4_vehicle_msgs.msg import ActuationCommandStamped + +COUNTDOWN_TIME = 3 # [sec] +TARGET_VELOCITY = 42.5 / 4 # [km/h] +TARGET_ACTUATION_FOR_ACCEL = 0.3 +TARGET_ACTUATION_FOR_BRAKE = 0.5 +TARGET_JERK_FOR_DRIVE = 1.5 # [m/s^3] +TARGET_JERK_FOR_BRAKE = -1.5 # [m/s^3] + +MAX_ACCEL_PEDAL = 0.5 +MIN_BRAKE_PEDAL = 0.8 + +TOPIC_LIST_FOR_VALIDATION = [ + "/vehicle/status/velocity_status", + "/control/command/actuation_cmd", + "/sensing/imu/imu_data", + "/vehicle/status/control_mode", +] + +NODE_LIST_FOR_VALIDATION = [ + "/raw_vehicle_cmd_converter" +] + + +class MapAccuracyTester(Node): + def __init__(self): + super().__init__("map_accuracy_tester") + 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 actuate tester =====") + lib.system.check_service_active("autoware.service") + lib.system.check_node_active(NODE_LIST_FOR_VALIDATION) + + print("===== Reset commands =====") + lib.command.reset_commands(self) + + print("===== Start checking accel map =====") + lib.cui.do_check("Do you want to accel pedal data?", lambda: self.check("accel")) + + print("===== Start checking brake map =====") + lib.cui.do_check("Do you want to brake pedal 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 + " actuation", 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 {TARGET_VELOCITY} km/h with accel pedal actuation {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, TARGET_VELOCITY, break_time=30.0) + print("===== End rosbag record =====") + process.terminate() + lib.command.actuate( + self, "brake", TARGET_ACTUATION_FOR_BRAKE, 1e-3 + ) + elif mode == "brake": + print( + f"===== Drive to {TARGET_VELOCITY} km/h and brake pedal actuation 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", + TARGET_ACTUATION_FOR_ACCEL, + 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, TOPIC_LIST_FOR_VALIDATION) + if not is_rosbag_valid: + print(f"Rosag 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, MAX_ACCEL_PEDAL + if mode == "brake": + return 0.0, MIN_BRAKE_PEDAL + + def get_rosbag_name(self, mode, target_acceleration): + current_time = datetime.now().strftime("%Y%m%d-%H%M%S") + filename = "_".join( + [ + "acceleration_accuracy", + mode, + str(target_acceleration), + current_time, + ] + ) + return filename + + +def main(args=None): + rclpy.init(args=args) + + tester = MapAccuracyTester() + 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..148ef3cb --- /dev/null +++ b/control_data_collecting_tool/scripts/calibration/lib/command.py @@ -0,0 +1,197 @@ +#!/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 == None: + acceleration_cmd = target_acceleration + else: + acceleration_cmd = 0.0 + + condition = ( + lambda: acceleration_cmd < target_acceleration - 1e-3 + if mode == "drive" + else acceleration_cmd > target_acceleration + 1e-3 + ) + while condition(): + 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 * 3.6 >= target_velocity) or ( + mode == "brake" and node.current_velocity * 3.6 <= target_velocity + ): + print(f"Reached {target_velocity} km/h.") + 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} command: {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 * 3.6 >= target_velocity) + or (mode == "brake" and node.current_velocity * 3.6 <= target_velocity) + ): + print(f"Reached {target_velocity} km/h.") + 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..4f3002dd --- /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.") + 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 occured 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.") + 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/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..0d279617 100755 --- a/control_data_collecting_tool/scripts/data_collecting_data_counter.py +++ b/control_data_collecting_tool/scripts/data_collecting_data_counter.py @@ -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 ) @@ -239,6 +255,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 +303,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 +312,8 @@ def timer_callback_counter(self): self.count_observations( current_vel, current_acc, current_steer, current_steer_rate, current_jerk ) + if abs(current_steer) < 0.2: + 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 +341,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 57d01bdb..524ed62e 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_input") + + # 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", @@ -181,6 +193,14 @@ def __init__(self): ) self.sub_trajectory_ + self.sub_acceleration_cmd_ = self.create_subscription( + Float32, + "/data_collecting_acceleration_cmd", + self.onAccelerationCmd, + 1, + ) + self.sub_acceleration_cmd_ + self.sub_operation_mode_ = self.create_subscription( OperationModeState, "/system/operation_mode/state", @@ -221,6 +241,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 +258,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 +499,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..82bbea9c --- /dev/null +++ b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_actuation_cmd.py @@ -0,0 +1,669 @@ +#!/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]" + ), + ) + + # 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, + 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.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, + ): + # naive pure pursuit steering control law + wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value + # acc_kp = self.get_parameter("acc_kp").get_parameter_value().double_value + # longitudinal_vel_err = longitudinal_vel_obs - longitudinal_vel_ref_nearest + # ure_pursuit_acc_cmd = -acc_kp * longitudinal_vel_err + + 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, + ): + # control law equal to simple_trajectory_follower in autoware + wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value + # acc_kp = self.get_parameter("acc_kp").get_parameter_value().double_value + + # Currently, the following params are not 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_rosbag_record.py b/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py index f6188c49..5a49e2ab 100755 --- a/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py +++ b/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py @@ -85,13 +85,9 @@ def subscribe_topics(self): # Create topics in the rosbag for recording if self.open_writer: - for topic_name, topic_type in zip( - subscribable_topic_list, subscribable_topic_type_list - ): + for topic_name, topic_type in zip(subscribable_topic_list, subscribable_topic_type_list): if topic_name not in self.subscribed_topic: - self.node.get_logger().info( - f"Recording topic: {topic_name} of type: {topic_type}" - ) + self.node.get_logger().info(f"Recording topic: {topic_name} of type: {topic_type}") topic_metadata = TopicMetadata( name=topic_name, type=topic_type, serialization_format="cdr" ) @@ -122,6 +118,7 @@ def record(self): if topic_name in self.subscribing_topic: self.subscribing_topic.remove(topic_name) + # call back function called in start recording def callback_write_message(self, topic_name, message): try: @@ -197,14 +194,15 @@ def record_message(self): self.present_operation_mode_ == 3 and self._present_control_mode_ == 1 and self.subscribed - ): + ): self.writer.subscribe_topics() self.writer.record() - + # Stop recording if the operation mode changes from 3(LOCAL) if ( - self.present_operation_mode_ != 3 or self._present_control_mode_ != 1 - ) and self.subscribed: + (self.present_operation_mode_ != 3 or self._present_control_mode_ != 1) + and self.subscribed + ): self.writer.stop_record() self.subscribed = False @@ -220,4 +218,4 @@ def main(args=None): if __name__ == "__main__": - main() + main() \ No newline at end of file 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 e7f91c32..da23b9f0 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -14,6 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from accel_brake_map import AccelBrakeMapConverter from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint from courses.load_course import declare_course_params @@ -29,6 +30,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 +156,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 +252,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 +286,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() @@ -510,21 +552,41 @@ 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]] + if self.present_operation_mode_ == 3: + self.nearestIndex = index_range[index_array_near[0]] # set target velocity 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": + 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, + ) + 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 +673,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 +714,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 +784,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 +815,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 +860,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..0d95ed21 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"] \ No newline at end of file From 208da26dee91796537bc8bde13a58f5e4f9a95ae Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Fri, 27 Dec 2024 06:45:07 +0900 Subject: [PATCH 02/17] Temporarily added data_collecting_pure_pursuit_trajectory_follower.py Signed-off-by: Yoshihiro Kogure --- ...ecting_pure_pursuit_trajectory_follower.py | 630 ++++++++++++++++++ 1 file changed, 630 insertions(+) create mode 100644 control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py diff --git a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py new file mode 100644 index 00000000..d82a3731 --- /dev/null +++ b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py @@ -0,0 +1,630 @@ +#!/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 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 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 DataCollectingPurePursuitTrajectoryFollower(Node): + def __init__(self): + super().__init__("data_collecting_pure_pursuit_trajectory_follower") + + 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]" + ), + ) + + # 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, + 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.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_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, + ) + + 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.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 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_control( + self, + pos_xy_obs, + pos_yaw_obs, + longitudinal_vel_obs, + pos_xy_ref_target, + longitudinal_vel_ref_nearest, + ): + # naive pure pursuit steering control law + wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value + acc_kp = self.get_parameter("acc_kp").get_parameter_value().double_value + longitudinal_vel_err = longitudinal_vel_obs - longitudinal_vel_ref_nearest + pure_pursuit_acc_cmd = -acc_kp * longitudinal_vel_err + + 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) + + return np.array([pure_pursuit_acc_cmd, steer]) + + def linearized_pure_pursuit_control( + self, + pos_xy_obs, + pos_yaw_obs, + longitudinal_vel_obs, + pos_xy_ref_nearest, + pos_yaw_ref_nearest, + longitudinal_vel_ref_nearest, + ): + # control law equal to simple_trajectory_follower in autoware + wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value + acc_kp = self.get_parameter("acc_kp").get_parameter_value().double_value + + # Currently, the following params are not 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 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 + ) + + cmd = np.zeros(2) + if pure_pursuit_type == "naive": + cmd = self.pure_pursuit_control( + present_position[:2], + present_yaw, + present_linear_velocity[0], + trajectory_position[targetIndex][:2], + trajectory_longitudinal_velocity[nearestIndex], + ) + elif pure_pursuit_type == "linearized": + cmd = self.linearized_pure_pursuit_control( + present_position[:2], + present_yaw, + present_linear_velocity[0], + trajectory_position[targetIndex][:2], + getYaw(trajectory_orientation[targetIndex]), + trajectory_longitudinal_velocity[nearestIndex], + ) + else: + self.get_logger().info( + 'pure_pursuit_type should be "naive" or "linearized" but is set to "%s" ' + % pure_pursuit_type + ) + + 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 cmd + 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 = control_cmd_msg.lateral.stamp + 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 = DataCollectingPurePursuitTrajectoryFollower() + + rclpy.spin(data_collecting_pure_pursuit_trajectory_follower) + + data_collecting_pure_pursuit_trajectory_follower.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file From c539e81c186cc6074e8033ffbd185f8789dd26f8 Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Fri, 27 Dec 2024 06:48:51 +0900 Subject: [PATCH 03/17] Remove data_collecting_pure_pursuit_trajectory_follower.py Signed-off-by: Yoshihiro Kogure --- ...ecting_pure_pursuit_trajectory_follower.py | 630 ------------------ 1 file changed, 630 deletions(-) delete mode 100644 control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py diff --git a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py deleted file mode 100644 index d82a3731..00000000 --- a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py +++ /dev/null @@ -1,630 +0,0 @@ -#!/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 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 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 DataCollectingPurePursuitTrajectoryFollower(Node): - def __init__(self): - super().__init__("data_collecting_pure_pursuit_trajectory_follower") - - 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]" - ), - ) - - # 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, - 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.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_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, - ) - - 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.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 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_control( - self, - pos_xy_obs, - pos_yaw_obs, - longitudinal_vel_obs, - pos_xy_ref_target, - longitudinal_vel_ref_nearest, - ): - # naive pure pursuit steering control law - wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value - acc_kp = self.get_parameter("acc_kp").get_parameter_value().double_value - longitudinal_vel_err = longitudinal_vel_obs - longitudinal_vel_ref_nearest - pure_pursuit_acc_cmd = -acc_kp * longitudinal_vel_err - - 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) - - return np.array([pure_pursuit_acc_cmd, steer]) - - def linearized_pure_pursuit_control( - self, - pos_xy_obs, - pos_yaw_obs, - longitudinal_vel_obs, - pos_xy_ref_nearest, - pos_yaw_ref_nearest, - longitudinal_vel_ref_nearest, - ): - # control law equal to simple_trajectory_follower in autoware - wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value - acc_kp = self.get_parameter("acc_kp").get_parameter_value().double_value - - # Currently, the following params are not 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 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 - ) - - cmd = np.zeros(2) - if pure_pursuit_type == "naive": - cmd = self.pure_pursuit_control( - present_position[:2], - present_yaw, - present_linear_velocity[0], - trajectory_position[targetIndex][:2], - trajectory_longitudinal_velocity[nearestIndex], - ) - elif pure_pursuit_type == "linearized": - cmd = self.linearized_pure_pursuit_control( - present_position[:2], - present_yaw, - present_linear_velocity[0], - trajectory_position[targetIndex][:2], - getYaw(trajectory_orientation[targetIndex]), - trajectory_longitudinal_velocity[nearestIndex], - ) - else: - self.get_logger().info( - 'pure_pursuit_type should be "naive" or "linearized" but is set to "%s" ' - % pure_pursuit_type - ) - - 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 cmd - 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 = control_cmd_msg.lateral.stamp - 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 = DataCollectingPurePursuitTrajectoryFollower() - - rclpy.spin(data_collecting_pure_pursuit_trajectory_follower) - - data_collecting_pure_pursuit_trajectory_follower.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() \ No newline at end of file From 7efc25c855f9b66133f9b6b92fa9c4ebc2d0c449 Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Mon, 30 Dec 2024 04:24:17 +0900 Subject: [PATCH 04/17] Add constant cmd scripts Signed-off-by: Yoshihiro Kogure --- control_data_collecting_tool/CMakeLists.txt | 2 + .../config/cmd_param.yaml | 38 ++++++++++ .../data_collecting_acceleration_cmd.py | 71 +++++++++---------- .../data_collecting_actuation_cmd.py | 59 ++++++++------- 4 files changed, 103 insertions(+), 67 deletions(-) create mode 100644 control_data_collecting_tool/config/cmd_param.yaml diff --git a/control_data_collecting_tool/CMakeLists.txt b/control_data_collecting_tool/CMakeLists.txt index 8039f457..cfc92ca5 100644 --- a/control_data_collecting_tool/CMakeLists.txt +++ b/control_data_collecting_tool/CMakeLists.txt @@ -39,6 +39,8 @@ install(PROGRAMS 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/config/cmd_param.yaml b/control_data_collecting_tool/config/cmd_param.yaml new file mode 100644 index 00000000..31b50204 --- /dev/null +++ b/control_data_collecting_tool/config/cmd_param.yaml @@ -0,0 +1,38 @@ +/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: 0.5 + TARGET_JERK_FOR_BRAKE: -0.5 + MIN_ACCEL: -6.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 + + +/data_collecting_actuation_cmd: + ros__parameters: + + # params for data_collecting_actuation_cmd + TARGET_ACTUATION_FOR_ACCEL: 0.3 + TARGET_ACTUATION_FOR_BRAKE: 0.5 + TARGET_JERK_FOR_DRIVE: 1.5 + TARGET_JERK_FOR_BRAKE: -1.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/scripts/calibration/data_collecting_acceleration_cmd.py b/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py index 67ff8abb..ff2e80c2 100755 --- a/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py @@ -16,8 +16,11 @@ from datetime import datetime import sys +import os +import yaml from std_msgs.msg import Float32 +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 @@ -33,30 +36,26 @@ from tier4_vehicle_msgs.msg import ActuationCommandStamped COUNTDOWN_TIME = 3 # [sec] -TARGET_VELOCITY = 42.5 # [km/h] -TARGET_ACCELERATION_FOR_DRIVE = 1.5 # [m/s^2] -TARGET_ACCELERATION_FOR_BRAKE = -1.5 # [m/s^2] -TARGET_JERK_FOR_DRIVE = 0.5 # [m/s^3] -TARGET_JERK_FOR_BRAKE = -0.5 # [m/s^3] -MIN_ACCEL = -6.0 -MAX_ACCEL = 2.0 -MIN_ACCEL_SUB_BRAKE = -2.0 - -TOPIC_LIST_FOR_VALIDATION = [ - "/control/command/control_cmd", - "/vehicle/status/velocity_status", - "/vehicle/status/control_mode", -] - -NODE_LIST_FOR_VALIDATION = [ - "/raw_vehicle_cmd_converter" -] - - -class MapAccuracyTester(Node): +class DataCollectingAccelerationCmd(Node): def __init__(self): - super().__init__("map_accuracy_tester") + 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", "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" ) @@ -102,11 +101,11 @@ def on_gear_status(self, msg): def run(self): print("===== Start map accuracy tester =====") lib.system.check_service_active("autoware.service") - lib.system.check_node_active(NODE_LIST_FOR_VALIDATION) + lib.system.check_node_active(self.NODE_LIST_FOR_VALIDATION) - print(f"===== Set Acceleration to {TARGET_ACCELERATION_FOR_BRAKE} =====") + print(f"===== Set Acceleration to {self.TARGET_ACCELERATION_FOR_BRAKE} =====") lib.command.accelerate( - self, TARGET_ACCELERATION_FOR_BRAKE, 1e-3, "brake", TARGET_JERK_FOR_BRAKE + self, self.TARGET_ACCELERATION_FOR_BRAKE, 1e-3, "brake", self.TARGET_JERK_FOR_BRAKE ) print("===== Start checking accel map =====") @@ -128,7 +127,7 @@ def check(self, mode): if mode == "accel": print( - f"===== Drive to {TARGET_VELOCITY} km/h with acceleration {target_acceleration} =====" + f"===== Drive to {self.TARGET_VELOCITY} km/h with acceleration {target_acceleration} =====" ) lib.command.change_gear(self, "drive") lib.cui.ready_check("Ready to drive?") @@ -137,11 +136,11 @@ def check(self, mode): 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, TARGET_VELOCITY, "drive",break_time=60.0) + lib.command.accelerate(self, target_acceleration, self.TARGET_VELOCITY, "drive",break_time=60.0) print("===== End rosbag record =====") process.terminate() lib.command.accelerate( - self, TARGET_ACCELERATION_FOR_BRAKE, 1e-3, "brake", TARGET_JERK_FOR_BRAKE + self, self.TARGET_ACCELERATION_FOR_BRAKE, 1e-3, "brake", self.TARGET_JERK_FOR_BRAKE ) elif mode == "brake": @@ -153,10 +152,10 @@ def check(self, mode): lib.cui.countdown(COUNTDOWN_TIME) lib.command.accelerate( self, - TARGET_ACCELERATION_FOR_DRIVE, - TARGET_VELOCITY, + self.TARGET_ACCELERATION_FOR_DRIVE, + self.TARGET_VELOCITY, "drive", - TARGET_JERK_FOR_DRIVE, + self.TARGET_JERK_FOR_DRIVE, ) print("===== Record rosbag =====") filename = self.get_rosbag_name(mode, target_acceleration) @@ -172,7 +171,7 @@ def check(self, mode): process.wait() print("===== Validate rosbag =====") - is_rosbag_valid = lib.rosbag.validate(filename, TOPIC_LIST_FOR_VALIDATION) + is_rosbag_valid = lib.rosbag.validate(filename, self.TOPIC_LIST_FOR_VALIDATION) if not is_rosbag_valid: print(f"Rosag validation error: {filename}") sys.exit(1) @@ -183,11 +182,11 @@ def check(self, mode): def get_min_max_acceleration(self, mode): if mode == "accel": - return 0.0, MAX_ACCEL + return 0.0, self.MAX_ACCEL if mode == "brake": - return MIN_ACCEL, 0.0 + return self.MIN_ACCEL, 0.0 if mode == "sub_brake": - return MIN_ACCEL_SUB_BRAKE, 0.0 + 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") @@ -205,7 +204,7 @@ def get_rosbag_name(self, mode, target_acceleration): def main(args=None): rclpy.init(args=args) - tester = MapAccuracyTester() + tester = DataCollectingAccelerationCmd() tester.run() tester.destroy_node() 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 index 8002d939..6f743fbf 100755 --- a/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py @@ -16,8 +16,11 @@ from datetime import datetime import sys +import os +import yaml from std_msgs.msg import Float32 +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 @@ -33,30 +36,24 @@ from tier4_vehicle_msgs.msg import ActuationCommandStamped COUNTDOWN_TIME = 3 # [sec] -TARGET_VELOCITY = 42.5 / 4 # [km/h] -TARGET_ACTUATION_FOR_ACCEL = 0.3 -TARGET_ACTUATION_FOR_BRAKE = 0.5 -TARGET_JERK_FOR_DRIVE = 1.5 # [m/s^3] -TARGET_JERK_FOR_BRAKE = -1.5 # [m/s^3] -MAX_ACCEL_PEDAL = 0.5 -MIN_BRAKE_PEDAL = 0.8 - -TOPIC_LIST_FOR_VALIDATION = [ - "/vehicle/status/velocity_status", - "/control/command/actuation_cmd", - "/sensing/imu/imu_data", - "/vehicle/status/control_mode", -] +class DataCollectingActuationCmd(Node): + def __init__(self): + super().__init__("data_collecting_actuation_cmd") -NODE_LIST_FOR_VALIDATION = [ - "/raw_vehicle_cmd_converter" -] + package_share_directory = get_package_share_directory("control_data_collecting_tool") + topic_file_path = os.path.join(package_share_directory, "config", "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_ACTUATION_FOR_ACCEL = topic_data["data_collecting_acceleration_cmd"]["ros__parameters"]["TARGET_ACTUATION_FOR_ACCEL"] + self.TARGET_ACTUATION_FOR_BRAKE = topic_data["data_collecting_acceleration_cmd"]["ros__parameters"]["TARGET_ACTUATION_FOR_BRAKE"] + self.MAX_ACCEL_PEDAL = topic_data["data_collecting_acceleration_cmd"]["ros__parameters"]["MAX_ACCEL_PEDAL"] + self.MIN_BRAKE_PEDAL = topic_data["data_collecting_acceleration_cmd"]["ros__parameters"]["MIN_BRAKE_PEDAL"] + 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"] -class MapAccuracyTester(Node): - def __init__(self): - super().__init__("map_accuracy_tester") self.client_control_mode = self.create_client( ControlModeCommand, "/control/control_mode_request" ) @@ -112,7 +109,7 @@ def control_cmd_timer_callback(self): def run(self): print("===== Start actuate tester =====") lib.system.check_service_active("autoware.service") - lib.system.check_node_active(NODE_LIST_FOR_VALIDATION) + lib.system.check_node_active(self.NODE_LIST_FOR_VALIDATION) print("===== Reset commands =====") lib.command.reset_commands(self) @@ -142,20 +139,20 @@ def check(self, mode): print(f"record rosbag: {filename}") print( - f"===== Drive to {TARGET_VELOCITY} km/h with accel pedal actuation {target_actuation} =====" + f"===== Drive to {self.TARGET_VELOCITY} km/h with accel pedal actuation {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, TARGET_VELOCITY, break_time=30.0) + 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", TARGET_ACTUATION_FOR_BRAKE, 1e-3 + self, "brake", self.TARGET_ACTUATION_FOR_BRAKE, 1e-3 ) elif mode == "brake": print( - f"===== Drive to {TARGET_VELOCITY} km/h and brake pedal actuation with {target_actuation} =====" + f"===== Drive to {self.TARGET_VELOCITY} km/h and brake pedal actuation with {target_actuation} =====" ) lib.command.change_gear(self, "drive") lib.cui.ready_check("Ready to drive?") @@ -163,8 +160,8 @@ def check(self, mode): lib.command.actuate( self, "accel", - TARGET_ACTUATION_FOR_ACCEL, - TARGET_VELOCITY, + 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) @@ -181,7 +178,7 @@ def check(self, mode): process.wait() print("===== Validate rosbag =====") - is_rosbag_valid = lib.rosbag.validate(filename, TOPIC_LIST_FOR_VALIDATION) + is_rosbag_valid = lib.rosbag.validate(filename, self.TOPIC_LIST_FOR_VALIDATION) if not is_rosbag_valid: print(f"Rosag validation error: {filename}") sys.exit(1) @@ -192,9 +189,9 @@ def check(self, mode): def get_min_max_acceleration(self, mode): if mode == "accel": - return 0.0, MAX_ACCEL_PEDAL + return 0.0, self.MAX_ACCEL_PEDAL if mode == "brake": - return 0.0, MIN_BRAKE_PEDAL + 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") @@ -212,7 +209,7 @@ def get_rosbag_name(self, mode, target_acceleration): def main(args=None): rclpy.init(args=args) - tester = MapAccuracyTester() + tester = DataCollectingActuationCmd() tester.run() tester.destroy_node() From ceeaefaa7765d727f08d1355801e43a5c3d83ef4 Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Mon, 30 Dec 2024 04:25:50 +0900 Subject: [PATCH 05/17] Add pedal threshold for data counting Signed-off-by: Yoshihiro Kogure --- .../config/common_param.yaml | 4 +++- .../scripts/data_collecting_data_counter.py | 14 ++++++++++++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/control_data_collecting_tool/config/common_param.yaml b/control_data_collecting_tool/config/common_param.yaml index 64cec602..1013d5ff 100644 --- a/control_data_collecting_tool/config/common_param.yaml +++ b/control_data_collecting_tool/config/common_param.yaml @@ -2,7 +2,7 @@ ros__parameters: CONTROL_MODE: acceleration_cmd # CONTROL_MODE: actuation_cmd - #CONTROL_MODE: external_acceleration_cmd + # CONTROL_MODE: external_acceleration_cmd # CONTROL_MODE: external_actuation_cmd LOAD_ROSBAG2_FILES: false @@ -36,6 +36,8 @@ 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 VEL_STEER_THRESHOLD: 20 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 0d279617..9226cdb0 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, @@ -121,6 +121,16 @@ 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))] @@ -312,7 +322,7 @@ def timer_callback_counter(self): self.count_observations( current_vel, current_acc, current_steer, current_steer_rate, current_jerk ) - if abs(current_steer) < 0.2: + 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)) From 24dc0b56e3d4aae6e00980ebcd0d65ebe1a043ca Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Mon, 30 Dec 2024 18:01:13 +0900 Subject: [PATCH 06/17] pre-commit run Signed-off-by: Yoshihiro Kogure --- control_data_collecting_tool/README.md | 66 ++++++++++---- .../control_data_collecting_tool.launch.py | 10 +-- .../data_collecting_acceleration_cmd.py | 87 +++++++++++++------ .../data_collecting_actuation_cmd.py | 69 ++++++++++----- .../scripts/calibration/lib/command.py | 38 ++++---- .../scripts/calibration/lib/cui.py | 6 +- .../scripts/data_collecting_data_counter.py | 18 +++- ...it_trajectory_follower_acceleration_cmd.py | 11 +-- ...rsuit_trajectory_follower_actuation_cmd.py | 36 ++++---- .../scripts/data_collecting_rosbag_record.py | 20 +++-- .../data_collecting_trajectory_publisher.py | 1 - .../scripts/params.py | 2 +- 12 files changed, 240 insertions(+), 124 deletions(-) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 57644e31..37e33d9d 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,29 @@ 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`, `actuation_cmd`], please proceed to 7.1. If you select the control mode from [`external_acceleration_cmd`, `external_actuation_cmd`], please proceed to 7.2. + + - 7.1 If you choose the control mode from [ `acceleration_cmd`, `actuation_cmd`], click the `LOCAL` button in `AutowareStatePanel`. + + + + Then, data collecting starts. - + - 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.) - + - 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 In the case you choose the control mode from [`external_acceleration_cmd`, `external_actuation_cmd`]. - 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. + - `external_acceleration_cmd` + + - `external_actuation_cmd` 8. If you want to stop data collecting automatic driving, run the following command @@ -208,7 +217,7 @@ You can create an original mask to specify the data collection range for the hea ```Python3 # sine_curve is derived from appropriate amplitude and period, defined separately - target_velocity = target_velocity_on_section + sine_curve + sine_curve + target_velocity = target_velocity_on_section + sine_curve ``` 4. Deceleration phase @@ -275,8 +284,6 @@ 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`): @@ -290,7 +297,7 @@ ROS 2 parameters which are common in all trajectories (`/config/common_param.yam | `NUM_BINS_STEER` | `int` | Number of bins of steer in heatmap | 20 | | `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_JERK` | `int` | Number of bins of jerk in heatmap ` | 10 | | `V_MIN` | `double` | Minimum velocity in heatmap [m/s] | 0.0 | | `V_MAX` | `double` | Maximum velocity in heatmap [m/s] | 11.5 | | `STEER_MIN` | `double` | Minimum steer in heatmap [rad] | -0.6 | @@ -302,6 +309,7 @@ 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 | +| `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 | @@ -386,3 +394,29 @@ 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` | | 11.80 | +| `TARGET_ACCELERATION_FOR_DRIVE` | `double` | | 0.3 | +| `TARGET_ACCELERATION_FOR_BRAKE` | `double` | | 0.5 | +| `TARGET_JERK_FOR_DRIVE` | `double` | | 1.5 | +| `TARGET_JERK_FOR_BRAKE` | `double` | | -1.5 | +| `MIN_ACCEL` | `double` | | -5.0 | +| `MAX_ACCEL` | `double` | | 2.0 | + +- `data_collecting_actuation_cmd.py` + +| Name | Type | Description | Default value | +| :--------------------------- | :------- | :---------- | :------------ | +| `TARGET_VELOCITY` | `double` | | 11.80 | +| `TARGET_ACTUATION_FOR_ACCEL` | `double` | | 0.3 | +| `TARGET_ACTUATION_FOR_BRAKE` | `double` | | 0.5 | +| `TARGET_JERK_FOR_DRIVE` | `double` | | 1.5 | +| `TARGET_JERK_FOR_BRAKE` | `double` | | -1.5 | +| `MAX_ACCEL_PEDAL` | `double` | | 0.5 | +| `MIN_BRAKE_PEDAL` | `double` | | 0.8 | 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 6b747499..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 @@ -115,10 +115,7 @@ def generate_launch_description(): ] ) - elif ( - control_mode == "actuation_cmd" - or control_mode == "external_actuation_cmd" - ): + elif control_mode == "actuation_cmd" or control_mode == "external_actuation_cmd": return launch.LaunchDescription( [ map_path_arg, @@ -126,7 +123,10 @@ def generate_launch_description(): 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}], + parameters=[ + common_param_file_path, + {"accel_brake_map_path": accel_brake_map_path}, + ], ), Node( package="control_data_collecting_tool", 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 index ff2e80c2..76f96dad 100755 --- a/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py @@ -15,11 +15,9 @@ # limitations under the License. from datetime import datetime -import sys import os -import yaml +import sys -from std_msgs.msg import Float32 from ament_index_python.packages import get_package_share_directory from autoware_control_msgs.msg import Control from autoware_vehicle_msgs.msg import ControlModeReport @@ -33,28 +31,53 @@ 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", "cmd_param.yaml") + 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.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"] + + print(self.NODE_LIST_FOR_VALIDATION) self.client_control_mode = self.create_client( ControlModeCommand, "/control/control_mode_request" @@ -63,7 +86,9 @@ def __init__(self): 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_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) @@ -99,7 +124,7 @@ def on_gear_status(self, msg): self.current_gear = msg.report def run(self): - print("===== Start map accuracy tester =====") + print("===== Start data_collecting_acceleration_cmd =====") lib.system.check_service_active("autoware.service") lib.system.check_node_active(self.NODE_LIST_FOR_VALIDATION) @@ -108,11 +133,15 @@ def run(self): self, self.TARGET_ACCELERATION_FOR_BRAKE, 1e-3, "brake", self.TARGET_JERK_FOR_BRAKE ) - print("===== Start checking accel map =====") - lib.cui.do_check("Do you want to check accel map?", lambda: self.check("accel")) + print("===== Start collecting constant positive acceleration cmd data =====") + lib.cui.do_check( + "Do you want to collect positive acceleration cmd data?", lambda: self.check("accel") + ) - print("===== Start checking brake map =====") - lib.cui.do_check("Do you want to check brake map?", lambda: self.check("brake")) + print("===== Start collecting constant negative acceleration cmd data =====") + lib.cui.do_check( + "Do you want to collect negative acceleration cmd data?", lambda: self.check("brake") + ) print("===== Successfully finished! =====") @@ -127,7 +156,7 @@ def check(self, mode): if mode == "accel": print( - f"===== Drive to {self.TARGET_VELOCITY} km/h with acceleration {target_acceleration} =====" + 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?") @@ -136,16 +165,22 @@ def check(self, mode): 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) + 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 + self, + self.TARGET_ACCELERATION_FOR_BRAKE, + 1e-3, + "brake", + self.TARGET_JERK_FOR_BRAKE, ) - + elif mode == "brake": print( - f"===== Drive to {TARGET_VELOCITY} km/h and brake with {target_acceleration} =====" + 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?") @@ -164,7 +199,7 @@ def check(self, mode): lib.command.accelerate(self, target_acceleration, 1e-3, "brake") print("===== End rosbag record =====") process.terminate() - + else: print(f"Invalid mode: {mode}") sys.exit(1) @@ -173,7 +208,7 @@ def check(self, mode): print("===== Validate rosbag =====") is_rosbag_valid = lib.rosbag.validate(filename, self.TOPIC_LIST_FOR_VALIDATION) if not is_rosbag_valid: - print(f"Rosag validation error: {filename}") + print(f"Rosbag validation error: {filename}") sys.exit(1) is_finished = lib.cui.finish_check(f"Will you continue to check {mode} map?") 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 index 6f743fbf..2ec3c81e 100755 --- a/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py @@ -15,11 +15,9 @@ # limitations under the License. from datetime import datetime -import sys import os -import yaml +import sys -from std_msgs.msg import Float32 from ament_index_python.packages import get_package_share_directory from autoware_control_msgs.msg import Control from autoware_vehicle_msgs.msg import ControlModeReport @@ -33,26 +31,47 @@ 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", "cmd_param.yaml") + 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_acceleration_cmd"]["ros__parameters"]["TARGET_VELOCITY"] - self.TARGET_ACTUATION_FOR_ACCEL = topic_data["data_collecting_acceleration_cmd"]["ros__parameters"]["TARGET_ACTUATION_FOR_ACCEL"] - self.TARGET_ACTUATION_FOR_BRAKE = topic_data["data_collecting_acceleration_cmd"]["ros__parameters"]["TARGET_ACTUATION_FOR_BRAKE"] - self.MAX_ACCEL_PEDAL = topic_data["data_collecting_acceleration_cmd"]["ros__parameters"]["MAX_ACCEL_PEDAL"] - self.MIN_BRAKE_PEDAL = topic_data["data_collecting_acceleration_cmd"]["ros__parameters"]["MIN_BRAKE_PEDAL"] - 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.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"] + + print(self.NODE_LIST_FOR_VALIDATION) self.client_control_mode = self.create_client( ControlModeCommand, "/control/control_mode_request" @@ -61,10 +80,12 @@ def __init__(self): 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_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 ) @@ -107,17 +128,17 @@ def control_cmd_timer_callback(self): self.pub_control_cmd.publish(control_cmd_msg) def run(self): - print("===== Start actuate tester =====") + 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 checking accel map =====") + print("===== Start collecting constant accel pedal data =====") lib.cui.do_check("Do you want to accel pedal data?", lambda: self.check("accel")) - print("===== Start checking brake map =====") + print("===== Start collecting constant brake pedal data =====") lib.cui.do_check("Do you want to brake pedal data?", lambda: self.check("brake")) print("===== Successfully finished! =====") @@ -130,7 +151,7 @@ def check(self, mode): target_actuation = lib.cui.input_target_value( mode + " actuation", min_actuation, max_actuation, "" ) - + if mode == "accel": print("===== Record rosbag =====") filename = self.get_rosbag_name(mode, target_actuation) @@ -139,20 +160,20 @@ def check(self, mode): print(f"record rosbag: {filename}") print( - f"===== Drive to {self.TARGET_VELOCITY} km/h with accel pedal actuation {target_actuation} =====" + f"===== Drive to {self.TARGET_VELOCITY} m/s with accel pedal actuation {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 + 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} km/h and brake pedal actuation with {target_actuation} =====" + f"===== Drive to {self.TARGET_VELOCITY} m/s and brake pedal actuation with {target_actuation} =====" ) lib.command.change_gear(self, "drive") lib.cui.ready_check("Ready to drive?") @@ -180,7 +201,7 @@ def check(self, mode): print("===== Validate rosbag =====") is_rosbag_valid = lib.rosbag.validate(filename, self.TOPIC_LIST_FOR_VALIDATION) if not is_rosbag_valid: - print(f"Rosag validation error: {filename}") + print(f"Rosbag validation error: {filename}") sys.exit(1) is_finished = lib.cui.finish_check(f"Will you continue to check {mode} map?") diff --git a/control_data_collecting_tool/scripts/calibration/lib/command.py b/control_data_collecting_tool/scripts/calibration/lib/command.py index 148ef3cb..2631ed89 100644 --- a/control_data_collecting_tool/scripts/calibration/lib/command.py +++ b/control_data_collecting_tool/scripts/calibration/lib/command.py @@ -94,24 +94,29 @@ def change_gear(node, target_gear): break -def accelerate(node, target_acceleration, target_velocity, mode, target_jerk=None, break_time=120.0): +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 == None: + + if target_jerk is None: acceleration_cmd = target_acceleration else: acceleration_cmd = 0.0 - condition = ( - lambda: acceleration_cmd < target_acceleration - 1e-3 - if mode == "drive" - else acceleration_cmd > target_acceleration + 1e-3 - ) - while condition(): + 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))) + node.pub_data_collecting_control_cmd.publish( + Float32(data=float(data_collecting_control_cmd)) + ) time.sleep(0.1) data_collecting_control_cmd = target_acceleration @@ -119,10 +124,10 @@ def accelerate(node, target_acceleration, target_velocity, mode, target_jerk=Non while rclpy.ok(): rclpy.spin_once(node) - if (mode == "drive" and node.current_velocity * 3.6 >= target_velocity) or ( - mode == "brake" and node.current_velocity * 3.6 <= target_velocity + if (mode == "drive" and node.current_velocity >= target_velocity) or ( + mode == "brake" and node.current_velocity <= target_velocity ): - print(f"Reached {target_velocity} km/h.") + 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)) @@ -153,11 +158,10 @@ def actuate(node, mode, target_command, target_velocity, break_time=120.0): while rclpy.ok(): rclpy.spin_once(node) - if ( - (mode == "accel" and node.current_velocity * 3.6 >= target_velocity) - or (mode == "brake" and node.current_velocity * 3.6 <= target_velocity) + if (mode == "accel" and node.current_velocity >= target_velocity) or ( + mode == "brake" and node.current_velocity <= target_velocity ): - print(f"Reached {target_velocity} km/h.") + print(f"Reached {target_velocity} m/s.") data_collecting_pedal_input = 0.0 if mode == "accel": data_collecting_pedal_input = 0.0 diff --git a/control_data_collecting_tool/scripts/calibration/lib/cui.py b/control_data_collecting_tool/scripts/calibration/lib/cui.py index 4f3002dd..19169965 100644 --- a/control_data_collecting_tool/scripts/calibration/lib/cui.py +++ b/control_data_collecting_tool/scripts/calibration/lib/cui.py @@ -35,7 +35,7 @@ def input_yes_or_no(description): else: print("Please enter 'yes' or 'no'.") except KeyboardInterrupt as e: - print("\nOperation canceled by user.") + print("\nOperation canceled by user : " + str(e)) sys.exit(1) except Exception as e: print(e) @@ -58,7 +58,7 @@ def do_check(description, func): try: func() except Exception as e: - print(f"Error occured while executing the function: {e}") + print(f"Error occurred while executing the function: {e}") def finish_check(description): @@ -79,7 +79,7 @@ def input_target_value(description, min_value, max_value, unit): continue break except KeyboardInterrupt as e: - print("\nOperation canceled by user.") + print("\nOperation canceled by user : " + str(e)) sys.exit(1) except Exception as e: print(e) 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 0d279617..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, @@ -121,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))] @@ -312,7 +326,7 @@ def timer_callback_counter(self): self.count_observations( current_vel, current_acc, current_steer, current_steer_rate, current_jerk ) - if abs(current_steer) < 0.2: + 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)) diff --git a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py index 524ed62e..3b8d00fb 100755 --- a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py +++ b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py @@ -116,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, @@ -195,7 +194,7 @@ def __init__(self): self.sub_acceleration_cmd_ = self.create_subscription( Float32, - "/data_collecting_acceleration_cmd", + "/data_collecting_accel_cmd", self.onAccelerationCmd, 1, ) @@ -506,6 +505,8 @@ def control(self): acceleration_cmd = self.acceleration_cmd cmd[0] = acceleration_cmd + self.get_logger().info("cmd_mode : " + self.CONTROL_MODE) + cmd_without_noise = 1 * cmd tmp_acc_noise = self.acc_noise_list.pop(0) @@ -557,9 +558,9 @@ def control(self): # [2] publish cmd 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.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] 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 index 82bbea9c..d9bca263 100755 --- 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 @@ -117,7 +117,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, @@ -178,6 +177,14 @@ def __init__(self): 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", @@ -235,10 +242,16 @@ def __init__(self): "/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" + 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 @@ -285,12 +298,6 @@ def pure_pursuit_steer_control( longitudinal_vel_obs, pos_xy_ref_target, ): - # naive pure pursuit steering control law - wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value - # acc_kp = self.get_parameter("acc_kp").get_parameter_value().double_value - # longitudinal_vel_err = longitudinal_vel_obs - longitudinal_vel_ref_nearest - # ure_pursuit_acc_cmd = -acc_kp * longitudinal_vel_err - 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] @@ -309,9 +316,8 @@ def linearized_pure_pursuit_steer_control( pos_xy_ref_nearest, pos_yaw_ref_nearest, ): - # control law equal to simple_trajectory_follower in autoware + wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value - # acc_kp = self.get_parameter("acc_kp").get_parameter_value().double_value # Currently, the following params are not declared as ROS 2 params. lookahead_coef = self.get_parameter("lookahead_time").get_parameter_value().double_value @@ -564,9 +570,9 @@ def control(self): # [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.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] diff --git a/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py b/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py index 5a49e2ab..f6188c49 100755 --- a/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py +++ b/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py @@ -85,9 +85,13 @@ def subscribe_topics(self): # Create topics in the rosbag for recording if self.open_writer: - for topic_name, topic_type in zip(subscribable_topic_list, subscribable_topic_type_list): + for topic_name, topic_type in zip( + subscribable_topic_list, subscribable_topic_type_list + ): if topic_name not in self.subscribed_topic: - self.node.get_logger().info(f"Recording topic: {topic_name} of type: {topic_type}") + self.node.get_logger().info( + f"Recording topic: {topic_name} of type: {topic_type}" + ) topic_metadata = TopicMetadata( name=topic_name, type=topic_type, serialization_format="cdr" ) @@ -118,7 +122,6 @@ def record(self): if topic_name in self.subscribing_topic: self.subscribing_topic.remove(topic_name) - # call back function called in start recording def callback_write_message(self, topic_name, message): try: @@ -194,15 +197,14 @@ def record_message(self): self.present_operation_mode_ == 3 and self._present_control_mode_ == 1 and self.subscribed - ): + ): self.writer.subscribe_topics() self.writer.record() - + # Stop recording if the operation mode changes from 3(LOCAL) if ( - (self.present_operation_mode_ != 3 or self._present_control_mode_ != 1) - and self.subscribed - ): + self.present_operation_mode_ != 3 or self._present_control_mode_ != 1 + ) and self.subscribed: self.writer.stop_record() self.subscribed = False @@ -218,4 +220,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + 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 b1af0c8d..8115dd1f 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -14,7 +14,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from accel_brake_map import AccelBrakeMapConverter from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint from courses.load_course import declare_course_params diff --git a/control_data_collecting_tool/scripts/params.py b/control_data_collecting_tool/scripts/params.py index 0d95ed21..cf050199 100644 --- a/control_data_collecting_tool/scripts/params.py +++ b/control_data_collecting_tool/scripts/params.py @@ -94,4 +94,4 @@ def __init__(self, param_dict): self.brake_pedal_input_bins[:-1] + self.brake_pedal_input_bins[1:] ) / 2 - self.control_mode = param_dict["CONTROL_MODE"] \ No newline at end of file + self.control_mode = param_dict["CONTROL_MODE"] From d600b26329cab4259396aa65e537eac84ac5805f Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Mon, 30 Dec 2024 18:03:54 +0900 Subject: [PATCH 07/17] Add some comments Signed-off-by: Yoshihiro Kogure --- .../scripts/accel_brake_map.py | 33 +++++++++++-------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/control_data_collecting_tool/scripts/accel_brake_map.py b/control_data_collecting_tool/scripts/accel_brake_map.py index 08496d6b..61cb1a9e 100644 --- a/control_data_collecting_tool/scripts/accel_brake_map.py +++ b/control_data_collecting_tool/scripts/accel_brake_map.py @@ -21,37 +21,38 @@ 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): @@ -62,18 +63,22 @@ def accel_input_to_pedal(self, accel_input, velocity): 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) - actuation_input = 0.0 + # 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]: - if accel_input_map[i] <= accel_input and accel_input <= 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] @@ -82,10 +87,10 @@ def accel_input_to_pedal(self, accel_input, velocity): 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 and accel_input <= accel_input_map[i]: + 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] @@ -100,25 +105,27 @@ def accel_input_to_pedal(self, accel_input, velocity): 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 ) From 90b0f752975731291b57f58fe9f3ecfbf9e50088 Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Mon, 30 Dec 2024 18:04:38 +0900 Subject: [PATCH 08/17] Update install programs Signed-off-by: Yoshihiro Kogure --- control_data_collecting_tool/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/control_data_collecting_tool/CMakeLists.txt b/control_data_collecting_tool/CMakeLists.txt index cfc92ca5..f665f7a7 100644 --- a/control_data_collecting_tool/CMakeLists.txt +++ b/control_data_collecting_tool/CMakeLists.txt @@ -33,7 +33,8 @@ 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 From 6547f55646d0a5c694c384f11e7742c51fbe79c4 Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Mon, 30 Dec 2024 18:05:46 +0900 Subject: [PATCH 09/17] Update params Signed-off-by: Yoshihiro Kogure --- .../config/cmd_param.yaml | 38 ------------------- .../config/common_param.yaml | 6 ++- .../acceleration_cmd_param.yaml | 18 +++++++++ .../actuation_cmd_param.yaml | 19 ++++++++++ .../config/topics.yaml | 6 +-- 5 files changed, 43 insertions(+), 44 deletions(-) delete mode 100644 control_data_collecting_tool/config/cmd_param.yaml create mode 100644 control_data_collecting_tool/config/constant_cmd_param/acceleration_cmd_param.yaml create mode 100644 control_data_collecting_tool/config/constant_cmd_param/actuation_cmd_param.yaml diff --git a/control_data_collecting_tool/config/cmd_param.yaml b/control_data_collecting_tool/config/cmd_param.yaml deleted file mode 100644 index 31b50204..00000000 --- a/control_data_collecting_tool/config/cmd_param.yaml +++ /dev/null @@ -1,38 +0,0 @@ -/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: 0.5 - TARGET_JERK_FOR_BRAKE: -0.5 - MIN_ACCEL: -6.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 - - -/data_collecting_actuation_cmd: - ros__parameters: - - # params for data_collecting_actuation_cmd - TARGET_ACTUATION_FOR_ACCEL: 0.3 - TARGET_ACTUATION_FOR_BRAKE: 0.5 - TARGET_JERK_FOR_DRIVE: 1.5 - TARGET_JERK_FOR_BRAKE: -1.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/common_param.yaml b/control_data_collecting_tool/config/common_param.yaml index 64cec602..82c65e1f 100644 --- a/control_data_collecting_tool/config/common_param.yaml +++ b/control_data_collecting_tool/config/common_param.yaml @@ -2,7 +2,7 @@ ros__parameters: CONTROL_MODE: acceleration_cmd # CONTROL_MODE: actuation_cmd - #CONTROL_MODE: external_acceleration_cmd + # CONTROL_MODE: external_acceleration_cmd # CONTROL_MODE: external_actuation_cmd LOAD_ROSBAG2_FILES: false @@ -36,6 +36,8 @@ 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 VEL_STEER_THRESHOLD: 20 @@ -61,4 +63,4 @@ lon_acc_lim: 1.5 lon_jerk_lim: 0.5 steer_lim: 0.6 - steer_rate_lim: 0.6 \ No newline at end of file + steer_rate_lim: 0.6 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..b208a734 --- /dev/null +++ b/control_data_collecting_tool/config/constant_cmd_param/acceleration_cmd_param.yaml @@ -0,0 +1,18 @@ +/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..aff58881 --- /dev/null +++ b/control_data_collecting_tool/config/constant_cmd_param/actuation_cmd_param.yaml @@ -0,0 +1,19 @@ +/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 + TARGET_JERK_FOR_DRIVE: 1.5 + TARGET_JERK_FOR_BRAKE: -1.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 \ No newline at end of file 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 From d3f5607296e4d687b03f856ffa9db5ec76d9a0d9 Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Mon, 30 Dec 2024 18:11:59 +0900 Subject: [PATCH 10/17] pre-commit run Signed-off-by: Yoshihiro Kogure --- .../config/constant_cmd_param/acceleration_cmd_param.yaml | 1 - .../config/constant_cmd_param/actuation_cmd_param.yaml | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) 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 index b208a734..b485f06f 100644 --- 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 @@ -1,6 +1,5 @@ /data_collecting_acceleration_cmd: ros__parameters: - # params for data_collecting_acceleration_cmd TARGET_VELOCITY: 11.80 TARGET_ACCELERATION_FOR_DRIVE: 1.5 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 index aff58881..303a768f 100644 --- 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 @@ -1,10 +1,9 @@ /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 + TARGET_ACTUATION_FOR_BRAKE: 0.5 TARGET_JERK_FOR_DRIVE: 1.5 TARGET_JERK_FOR_BRAKE: -1.5 MAX_ACCEL_PEDAL: 0.5 @@ -16,4 +15,4 @@ - /vehicle/status/control_mode validation_nodes: - /raw_vehicle_cmd_converter - - /data_collecting_pure_pursuit_trajectory_follower_actuation_cmd \ No newline at end of file + - /data_collecting_pure_pursuit_trajectory_follower_actuation_cmd From c6d1d93eacbad0b3656db08dbd7b9d8d723951af Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Mon, 30 Dec 2024 22:56:42 +0900 Subject: [PATCH 11/17] Update README Signed-off-by: Yoshihiro Kogure --- control_data_collecting_tool/README.md | 206 ++++++++++++++++-- .../actuation_cmd_param.yaml | 2 - .../data_collecting_acceleration_cmd.py | 15 +- .../data_collecting_actuation_cmd.py | 20 +- .../scripts/calibration/lib/command.py | 2 +- ...it_trajectory_follower_acceleration_cmd.py | 2 +- 6 files changed, 204 insertions(+), 43 deletions(-) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 37e33d9d..72d9c0e9 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -135,8 +135,180 @@ This package provides tools for automatically collecting data using pure pursuit - `external_acceleration_cmd` + This mode enables the collection of constant acceleration data for both positive and negative acceleration scenarios. + + - Positive Acceleration Data Collection + + 1. Start the Tool by using the following command + + ```bash + ros2 run control_data_collecting_tool data_collecting_acceleration_cmd + ``` + + 2. 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) + ``` + + 3. Input Acceleration: When prompted, input the desired acceleration value: + + ```bash + Target acceleration [0.0 ~ 2.0 m/s^2] + ``` + + 4. 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) + ``` + + 5. Execution and Recording: After 3-second counting, the vehicle accelerates to the 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/(.*) + ``` + + 6. Deceleration Phase: After reaching the target velocity, the tool applies a deceleration using the TARGET_ACCELERATION_FOR_BRAKE parameter to bring the vehicle to a stop. + + 7. 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 + + 1. Start the Tool by using the following command + + ```bash + ros2 run control_data_collecting_tool data_collecting_acceleration_cmd + ``` + + 2. 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) + ``` + + 3. Input Acceleration: When prompted, input the desired acceleration value: + + ```bash + Target acceleration [-5.0 ~ 0.0 m/s^2] + ``` + + 4. 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) + ``` + + 5. Acceleration Phase: After 3-second counting, the vehicle accelerates to the TARGET_VELOCITY with TARGET_ACCELERATION_FOR_DRIVE before braking. + + 6. Braking and Recording: Once the target velocity is achieved, the tool applies the braking command. During this process, you will see: + + ```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/(.*) + ``` + + 7. 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 + + 1. Start the tool by using the following command + + ```bash + ros2 run control_data_collecting_tool data_collecting_actuation_cmd + ``` + + 2. 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) + ``` + + 3. Input Accel Pedal input: When prompted, input the desired accel pedal input value: + + ```bash + Target accel pedal input [0.0 ~ 0.5 ] + ``` + + 4. 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) + ``` + + 5. Execution and Recording: After 3-second counting, the vehicle accelerates to the 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_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/(.*) + ``` + + 6. Deceleration Phase: After reaching the target velocity, the tool applies a deceleration using the TARGET_ACTUATION_FOR_BRAKE parameter to bring the vehicle to a stop. + + 7. 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 + + 1. Start the tool by using the following command + + ```bash + ros2 run control_data_collecting_tool data_collecting_actuation_cmd + ``` + + 2. 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) + ``` + + 3. Input Brake Pedal input: When prompted, input the desired brake pedal input value: + + ```bash + Target brake pedal input [0.0 ~ 0.8 ] + ``` + + 4. 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) + ``` + + 5. Acceleration Phase: After 3-second counting, the vehicle accelerates to the TARGET_VELOCITY with TARGET_ACTUATION_FOR_ACCEL before braking. + + 6. Braking and Recording: Once the 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_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/(.*) + ``` + + 7. 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 ```bash @@ -399,24 +571,22 @@ Each trajectory has specific ROS 2 parameters. - `data_collecting_acceleration_cmd.py` -| Name | Type | Description | Default value | -| :------------------------------ | :------- | :---------- | :------------ | -| `TARGET_VELOCITY` | `double` | | 11.80 | -| `TARGET_ACCELERATION_FOR_DRIVE` | `double` | | 0.3 | -| `TARGET_ACCELERATION_FOR_BRAKE` | `double` | | 0.5 | -| `TARGET_JERK_FOR_DRIVE` | `double` | | 1.5 | -| `TARGET_JERK_FOR_BRAKE` | `double` | | -1.5 | -| `MIN_ACCEL` | `double` | | -5.0 | -| `MAX_ACCEL` | `double` | | 2.0 | +| 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` | | 11.80 | -| `TARGET_ACTUATION_FOR_ACCEL` | `double` | | 0.3 | -| `TARGET_ACTUATION_FOR_BRAKE` | `double` | | 0.5 | -| `TARGET_JERK_FOR_DRIVE` | `double` | | 1.5 | -| `TARGET_JERK_FOR_BRAKE` | `double` | | -1.5 | -| `MAX_ACCEL_PEDAL` | `double` | | 0.5 | -| `MIN_BRAKE_PEDAL` | `double` | | 0.8 | +| 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/constant_cmd_param/actuation_cmd_param.yaml b/control_data_collecting_tool/config/constant_cmd_param/actuation_cmd_param.yaml index 303a768f..a4ab14c4 100644 --- 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 @@ -4,8 +4,6 @@ TARGET_VELOCITY: 11.80 TARGET_ACTUATION_FOR_ACCEL: 0.3 TARGET_ACTUATION_FOR_BRAKE: 0.5 - TARGET_JERK_FOR_DRIVE: 1.5 - TARGET_JERK_FOR_BRAKE: -1.5 MAX_ACCEL_PEDAL: 0.5 MIN_BRAKE_PEDAL: 0.8 topics: 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 index 76f96dad..39d521fa 100755 --- a/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_acceleration_cmd.py @@ -77,8 +77,6 @@ def __init__(self): "ros__parameters" ]["validation_nodes"] - print(self.NODE_LIST_FOR_VALIDATION) - self.client_control_mode = self.create_client( ControlModeCommand, "/control/control_mode_request" ) @@ -128,19 +126,16 @@ def run(self): lib.system.check_service_active("autoware.service") lib.system.check_node_active(self.NODE_LIST_FOR_VALIDATION) - print(f"===== Set Acceleration to {self.TARGET_ACCELERATION_FOR_BRAKE} =====") - lib.command.accelerate( - self, self.TARGET_ACCELERATION_FOR_BRAKE, 1e-3, "brake", self.TARGET_JERK_FOR_BRAKE - ) - print("===== Start collecting constant positive acceleration cmd data =====") lib.cui.do_check( - "Do you want to collect positive acceleration cmd data?", lambda: self.check("accel") + "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 negative acceleration cmd data?", lambda: self.check("brake") + "Do you want to collect constant negative acceleration cmd data?", + lambda: self.check("brake"), ) print("===== Successfully finished! =====") @@ -227,7 +222,7 @@ def get_rosbag_name(self, mode, target_acceleration): current_time = datetime.now().strftime("%Y%m%d-%H%M%S") filename = "_".join( [ - "acceleration_accuracy", + "constant_acceleration_cmd", mode, str(target_acceleration), current_time, 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 index 2ec3c81e..0abc1d22 100755 --- a/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py @@ -71,8 +71,6 @@ def __init__(self): "ros__parameters" ]["validation_nodes"] - print(self.NODE_LIST_FOR_VALIDATION) - self.client_control_mode = self.create_client( ControlModeCommand, "/control/control_mode_request" ) @@ -135,11 +133,11 @@ def run(self): print("===== Reset commands =====") lib.command.reset_commands(self) - print("===== Start collecting constant accel pedal data =====") - lib.cui.do_check("Do you want to accel pedal data?", lambda: self.check("accel")) + 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 data =====") - lib.cui.do_check("Do you want to brake pedal data?", lambda: self.check("brake")) + 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! =====") @@ -149,7 +147,7 @@ def check(self, mode): print("===== Input target accel pedal input =====") min_actuation, max_actuation = self.get_min_max_acceleration(mode) target_actuation = lib.cui.input_target_value( - mode + " actuation", min_actuation, max_actuation, "" + mode + " pedal input", min_actuation, max_actuation, "" ) if mode == "accel": @@ -160,7 +158,7 @@ def check(self, mode): print(f"record rosbag: {filename}") print( - f"===== Drive to {self.TARGET_VELOCITY} m/s with accel pedal actuation {target_actuation} =====" + 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?") @@ -173,7 +171,7 @@ def check(self, mode): 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 actuation with {target_actuation} =====" + 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?") @@ -204,7 +202,7 @@ def check(self, mode): print(f"Rosbag validation error: {filename}") sys.exit(1) - is_finished = lib.cui.finish_check(f"Will you continue to check {mode} map?") + is_finished = lib.cui.finish_check(f"Will you continue to collect {mode} pedal input data?") print(f"===== Successfully {mode} map checking finished! =====") @@ -218,7 +216,7 @@ def get_rosbag_name(self, mode, target_acceleration): current_time = datetime.now().strftime("%Y%m%d-%H%M%S") filename = "_".join( [ - "acceleration_accuracy", + "constant_actuation_cmd", mode, str(target_acceleration), current_time, diff --git a/control_data_collecting_tool/scripts/calibration/lib/command.py b/control_data_collecting_tool/scripts/calibration/lib/command.py index 2631ed89..3c45897f 100644 --- a/control_data_collecting_tool/scripts/calibration/lib/command.py +++ b/control_data_collecting_tool/scripts/calibration/lib/command.py @@ -142,7 +142,7 @@ def check_condition(acceleration_cmd, target_acceleration, mode): def actuate(node, mode, target_command, target_velocity, break_time=120.0): - print(f"Actuate with {mode} command: {target_command}.") + print(f"Actuate with {mode} pedal input: {target_command}.") start_time = time.time() data_collecting_pedal_input = 0.0 diff --git a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py index 3b8d00fb..c8ce777c 100755 --- a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py +++ b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py @@ -43,7 +43,7 @@ def getYaw(orientation_xyzw): class DataCollectingPurePursuitTrajectoryFollower(Node): def __init__(self): - super().__init__("data_collecting_pure_pursuit_trajectory_follower_acceleration_input") + super().__init__("data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd") # common params self.declare_parameter( From 4db8c223e827056f58cb87d0c4bddb10e042e841 Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Mon, 30 Dec 2024 23:03:15 +0900 Subject: [PATCH 12/17] The bullet points were changed from numbers to letters. Signed-off-by: Yoshihiro Kogure --- control_data_collecting_tool/README.md | 90 +++++++++++++------------- 1 file changed, 45 insertions(+), 45 deletions(-) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 72d9c0e9..9549810a 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -139,31 +139,31 @@ This package provides tools for automatically collecting data using pure pursuit - Positive Acceleration Data Collection - 1. Start the Tool by using the following command + a. Start the Tool by using the following command ```bash ros2 run control_data_collecting_tool data_collecting_acceleration_cmd ``` - 2. Confirm Data Collection: The tool prompts you to confirm whether to proceed with positive acceleration data collection: + 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) ``` - 3. Input Acceleration: When prompted, input the desired acceleration value: + c. Input Acceleration: When prompted, input the desired acceleration value: ```bash Target acceleration [0.0 ~ 2.0 m/s^2] ``` - 4. 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: + 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) ``` - 5. Execution and Recording: After 3-second counting, the vehicle accelerates to the target velocity specified in the configuration file. During this process, the following message is displayed: + f. Execution and Recording: After 3-second counting, the vehicle accelerates to the target velocity specified in the configuration file. During this process, the following message is displayed: ```bash Accelerate with target_acceleration m/s^2 @@ -175,39 +175,39 @@ This package provides tools for automatically collecting data using pure pursuit /control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*) ``` - 6. Deceleration Phase: After reaching the target velocity, the tool applies a deceleration using the TARGET_ACCELERATION_FOR_BRAKE parameter to bring the vehicle to a stop. + g. Deceleration Phase: After reaching the target velocity, the tool applies a deceleration using the TARGET_ACCELERATION_FOR_BRAKE parameter to bring the vehicle to a stop. - 7. Completion: Once the data is recorded and the vehicle is safely stopped, the session ends. The tool validates the recorded data. + 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 - 1. Start the Tool by using the following command + a. Start the Tool by using the following command ```bash ros2 run control_data_collecting_tool data_collecting_acceleration_cmd ``` - 2. 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): + 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) ``` - 3. Input Acceleration: When prompted, input the desired acceleration value: + c. Input Acceleration: When prompted, input the desired acceleration value: ```bash Target acceleration [-5.0 ~ 0.0 m/s^2] ``` - 4. 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: + 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) ``` - 5. Acceleration Phase: After 3-second counting, the vehicle accelerates to the TARGET_VELOCITY with TARGET_ACCELERATION_FOR_DRIVE before braking. + e. Acceleration Phase: After 3-second counting, the vehicle accelerates to the TARGET_VELOCITY with TARGET_ACCELERATION_FOR_DRIVE before braking. - 6. Braking and Recording: Once the target velocity is achieved, the tool applies the braking command. During this process, you will see: + f. Braking and Recording: Once the target velocity is achieved, the tool applies the braking command. During this process, you will see: ```bash Accelerate with target_acceleration m/s^2 @@ -219,7 +219,7 @@ This package provides tools for automatically collecting data using pure pursuit /control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*) ``` - 7. Completion: Once the data is recorded and the vehicle is safely stopped, the session ends. The tool validates the recorded data. + 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` @@ -227,31 +227,31 @@ This package provides tools for automatically collecting data using pure pursuit - Accel Pedal Data Collection - 1. Start the tool by using the following command + a. Start the tool by using the following command ```bash ros2 run control_data_collecting_tool data_collecting_actuation_cmd ``` - 2. Confirm Data Collection: The tool prompts you to confirm whether to proceed with positive acceleration data collection: + 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) ``` - 3. Input Accel Pedal input: When prompted, input the desired accel pedal input value: + c. Input Accel Pedal input: When prompted, input the desired accel pedal input value: ```bash - Target accel pedal input [0.0 ~ 0.5 ] + Target accel pedal input [0.0 ~ 0.5 ] ``` - 4. 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: + 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) ``` - 5. Execution and Recording: After 3-second counting, the vehicle accelerates to the target velocity specified in the configuration file. During this process, the following message is displayed: + e. Execution and Recording: After 3-second counting, the vehicle accelerates to the 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 @@ -263,39 +263,39 @@ This package provides tools for automatically collecting data using pure pursuit /control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*) ``` - 6. Deceleration Phase: After reaching the target velocity, the tool applies a deceleration using the TARGET_ACTUATION_FOR_BRAKE parameter to bring the vehicle to a stop. + f. Deceleration Phase: After reaching the target velocity, the tool applies a deceleration using the TARGET_ACTUATION_FOR_BRAKE parameter to bring the vehicle to a stop. - 7. Completion: Once the data is recorded and the vehicle is safely stopped, the session ends. The tool validates the recorded data. + 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 - 1. Start the tool by using the following command + a. Start the tool by using the following command ```bash ros2 run control_data_collecting_tool data_collecting_actuation_cmd ``` - 2. 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): + 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) ``` - 3. Input Brake Pedal input: When prompted, input the desired brake pedal input value: + c. Input Brake Pedal input: When prompted, input the desired brake pedal input value: ```bash Target brake pedal input [0.0 ~ 0.8 ] ``` - 4. 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: + 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) ``` - 5. Acceleration Phase: After 3-second counting, the vehicle accelerates to the TARGET_VELOCITY with TARGET_ACTUATION_FOR_ACCEL before braking. + e. Acceleration Phase: After 3-second counting, the vehicle accelerates to the TARGET_VELOCITY with TARGET_ACTUATION_FOR_ACCEL before braking. - 6. Braking and Recording: Once the target velocity is achieved, the tool applies the braking command. During this process, you will see: + f. Braking and Recording: Once the 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 @@ -307,7 +307,7 @@ This package provides tools for automatically collecting data using pure pursuit /control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*) ``` - 7. Completion: Once the data is recorded and the vehicle is safely stopped, the session ends. The tool validates the recorded data. + 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 @@ -571,22 +571,22 @@ Each trajectory has specific ROS 2 parameters. - `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 | +| 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 | +| 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 | From 08f6d3919a7c8eb82d183f01ea24c1574671e8fa Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Tue, 31 Dec 2024 11:51:02 +0900 Subject: [PATCH 13/17] Update print message Signed-off-by: Yoshihiro Kogure --- .../scripts/calibration/data_collecting_actuation_cmd.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 index 0abc1d22..6b968dbc 100755 --- a/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py +++ b/control_data_collecting_tool/scripts/calibration/data_collecting_actuation_cmd.py @@ -202,7 +202,9 @@ def check(self, mode): 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?") + is_finished = lib.cui.finish_check( + f"Will you continue to collect {mode} pedal input data?" + ) print(f"===== Successfully {mode} map checking finished! =====") From 8c120704cd04b0d56f26e4f9df8dc37d52010cb7 Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Thu, 2 Jan 2025 03:44:27 +0900 Subject: [PATCH 14/17] Fix bug and typo Signed-off-by: Yoshihiro Kogure --- control_data_collecting_tool/README.md | 48 ++--- .../resource/looking_ahead.png | Bin 17543 -> 17256 bytes .../scripts/courses/along_road.py | 4 +- .../scripts/courses/base_course.py | 40 ++++- .../scripts/courses/figure_eight.py | 3 +- .../scripts/courses/reversal_loop_circle.py | 167 ++++++++++++------ .../scripts/courses/straight_line_negative.py | 12 +- .../scripts/courses/straight_line_positive.py | 10 +- ...it_trajectory_follower_acceleration_cmd.py | 2 - .../data_collecting_trajectory_publisher.py | 38 ++-- 10 files changed, 206 insertions(+), 118 deletions(-) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 9549810a..01122513 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -113,9 +113,9 @@ 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. The following actions differ depending on the `CONTROL_MODE`. If you select the control mode from [ `acceleration_cmd`, `actuation_cmd`], please proceed to 7.1. If you select the control mode from [`external_acceleration_cmd`, `external_actuation_cmd`], please proceed to 7.2. +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`, `actuation_cmd`], click the `LOCAL` button in `AutowareStatePanel`. + - 7.1 If you choose the control mode from [ `acceleration_cmd`], click the `LOCAL` button in `AutowareStatePanel`. @@ -131,7 +131,13 @@ This package provides tools for automatically collecting data using pure pursuit 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 In the case you choose the control mode from [`external_acceleration_cmd`, `external_actuation_cmd`]. + - 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` @@ -163,7 +169,7 @@ This package provides tools for automatically collecting data using pure pursuit Ready to drive? (yes/no) ``` - f. Execution and Recording: After 3-second counting, the vehicle accelerates to the target velocity specified in the configuration file. During this process, the following message is displayed: + 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 @@ -175,7 +181,7 @@ This package provides tools for automatically collecting data using pure pursuit /control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*) ``` - g. Deceleration Phase: After reaching the target velocity, the tool applies a deceleration using the TARGET_ACCELERATION_FOR_BRAKE parameter to bring the vehicle to a stop. + 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. @@ -205,15 +211,15 @@ This package provides tools for automatically collecting data using pure pursuit Ready to drive? (yes/no) ``` - e. Acceleration Phase: After 3-second counting, the vehicle accelerates to the TARGET_VELOCITY with TARGET_ACCELERATION_FOR_DRIVE before braking. + 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 the target velocity is achieved, the tool applies the braking command. During this process, you will see: + f. Braking and Recording: Once `TARGET_VELOCITY` is achieved, the tool applies the braking command. During this process, you will see: ```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: + 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: ```bash /control/(.*)|/vehicle/(.*)|/imu/(.*)|/sensing/imu/(.*) @@ -251,19 +257,19 @@ This package provides tools for automatically collecting data using pure pursuit Ready to drive? (yes/no) ``` - e. Execution and Recording: After 3-second counting, the vehicle accelerates to the target velocity specified in the configuration file. During this process, the following message is displayed: + 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_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: + 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 the target velocity, the tool applies a deceleration using the TARGET_ACTUATION_FOR_BRAKE parameter to bring the vehicle to a stop. + 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. @@ -293,15 +299,15 @@ This package provides tools for automatically collecting data using pure pursuit 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. + 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 the target velocity is achieved, the tool applies the braking command. During this process, you will see: + 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_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: + 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/(.*) @@ -378,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. @@ -388,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 + # 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. @@ -448,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. diff --git a/control_data_collecting_tool/resource/looking_ahead.png b/control_data_collecting_tool/resource/looking_ahead.png index 10d0dcad376037b4bc308cc8c3c10e79cbc1d1cd..3c098291ab9729a1ee7913ff99dfd57435ee7914 100644 GIT binary patch literal 17256 zcmeHv^;eZa*EUE>cefxSEz(^ABA|evG}0Z1K6Hl?3eq3~(jiD3IwYhO>F&-$*ZJo7 zyzBkpi$CC7>vb(;A#?7zXV0E%UwiLsjC!fAgpWgmgMxyBucEA=iGp$u8wCY53>y>t zONtoo1^6cFrufE9%gNHs)6~@hMa|UB+1|;`-sT;XhlMM|#>r8DN05h~i^jy)yRZybTi9?hr>4 zm5-R}gIJ=Y!)YDO)0EVH4?OCBI_hd%E#}cjQ7vZSEw^?46H-U>br#{kN~#tMI2eZ_UwQfJ;!`c4Z)x+xVV3-|-`s&7Cc zCMI@~#=!w*;2qjLhoYYyLX^5JvMek z^oJZCACr^8QYZ{ivgEtdRx1HjHMDiIwKD zFjjimW=l4BxF`l^TJ}t*i(wAGKSTrWQu?=yGoZ6>HVOVSHy$Ag!TC(&< zMOoT($U}J!cjLV6pz&^&smET3w^}#n*Bfkm_=d5Pf?#!3DoahUVLsIp`XxO|DJB!|k)G)+W1b^CT>*c!H(LO*|%L+-GbHA2VP&m#H zjL-hGV>)L77rwCj?k#7nQ7GO<$F;epdva=cM<2@_k&C-@=oz1-S-+uci^**iGGdg5 z%KYz0>sDE%He=2p-1kr;*FwIuTVn^H-`J9C_<4V~YL6OoKI9a+z&8+kb-1BpV}Ti# zHb+jTpuie&PtI|R-P9pK`K{0CP9Ij<+zCDdmJ?#RsO(GClf~Y+%7X_GweaBb9-<4% z`b)LK`R~g8q{ImyWl@=Dw$u()dF!3|5O(%iBZv2`rBM4`KQ8aPYx=7&DL{VaZ!-V! zQKT4Qnrx2T+}AQS1%=tPd-qaWZ7O`)G8E&p+iFM7%nD~qFSZ+A@gNAG(n1rC_0F5q z6VNM}mG4AHP1mRKV57djlu^E6yb+T?Py=_Y?qOQ?K76#KuRF;rzid^xS5tDHXbvyK zC|d}bZjNqx3eb+%LLBKjLJ}Vl-mNd|_h_+g^?e1&BT7{(56mX-oFG@LHKFr_Wee_m zGB1dUIdm+9nRc(y$E7n6m)n_3wG{&n$*Ls-F_qVnknZl9?nho2GOIg8cT`?p6dHak zEXz+U1ejRw2*>f67l`jp!&6r9`})2qZS|P;_ghwU)L2St?_lp)1_sVX^yb@al(kZ=nY%9!49)@YEh*rS?TVpN`5>R7R}|vD+csV zKI@rU?(g5f|G0l{c^lgA9dK87!TuBqo7{ougKZwIUN4O(;b64JjMh6R(=Y^|<9hB# z*s-)V8>7xUzHJP=rc+eN z(|$CD+Rlg99mg0~5HV%3xP`jXidi7_sR8ZF8UprBhVobMqn}vbAH=_0Q$SjyN>sdX zKYIlK&HGxRqM#6MP(*+|)r|GlLx9+PirGUt)+iL%oi}?Ow&$`jlMN`KD;?C1@I14{8E{@RH_XvZZpWpX< z&ePn&f*;J@;I`FK_$v3fwB4}|0{*$$V^8nyycO}?tcPIUYXx6I61R?`y~?6rSy|cS z*GIO2bm6!lG^{HBTc5MNnPkWy_fffym0DbZ15{^eNmiW3v=Lp#I&zHCQ4Xtax zF59wDe%_7x&YvhTUEWH^@@9vaaW2k!K$3rzL`Frm_xHyL+KnH!V==~OWW3PUCQC?2 z_}SI<-FZn47^^#kQ0h%#_G<+NN0i(|y^B!|%=NrG2}ayj|MTMExajC7j*WY@87l(= zFLLF=aJm=ULm<;1KYhY)dsF9V*is?jb7H*`gvH=&3xy8~e*oLC0xZa9`?t_EL&Sw; zVS`uiNAKa`A)m+IbkfG9uCd>k^;A{9)=Zwtr}tpT*69f8#r|Rw5D4)YHmtfC#!zs- zpD==QZ(MXy8b6)>Ve`p}J2f@+nT$-}(F&n7Rrh{TIDd5$D%FD22UFiAFTB0T-*5^% zv$w*X%F-58;im7Y1-c`nqx-ze3JV`SKsw`TPaLDutoKHyxNC+DgCt?h8G1cnJx;kR6meC8$O*hi~C(^D*yO(u|odyBxNvJ}1vDWZvA| zTK#ksm-#Gt0DtQdGCBE%g4f7*Y~tw5!${?~WwmI{)Lja%O3y+eUgKH=-5fa#)tNw4 zw8>_uFs-no`QCI5otRss|LxUU2BVNY)zQ(>3vlFW>?W$fQ}3an(MxM2eO?bJQ8Ij` zRUY)E=-SjpoF`ko-crq@F_S^krxFY+=*g0VZvHJAeZ7E1Obq+}{NgNBCwy+`g0g#Y zOST0Sb$}u0Xr%|Tk?FbaJ7Qb`j==90Kj^bKhRo5=6HYA;p1uF_w%YPNjbL=b1EoJT zcDjI+9s3S~t#rOPH1#-`&$Qx|tFawZR#9o^d!LbQ2KED^SorDFMDi?B z8->N-709OgIX)$&S3~;tXGa(=DnzI)ij>_8oV%$yC&q(TM9b(vv>mmGGZVn$WdISD zBVS}cXK%oVrP-O8gHCrQ1e_OTRO6n#c|+Cjf0s&zfQpX#FkFnUo_!-q#?CGamgG3x z7CxLYW!ejbVLcnqm$37`IqDJhyLx(ieQwnmj=#ImnhhLA;N4H`s;Y9S#a93~N;S!e z-uF{dA>tBl=PX-~AqQz2y#qJSIWzkT~wK}l(}^sPYm zdun-bBo7ZCy$;Y;RG2j?(zEsc{WoY<@Ty-<+G(jH3=E`{RkYC7!L;|qIeRgPNDIUy z5@dWV^r*HG5oqsHvJp>bZCB`y@E}XaBJfss#?TT8c5Sh5cE&O{?tWjaqEC!HBQP=pbr-yiH zQx9EU3S~OwjRodk+LUu|fQ26U512~#v9MaJY9sK2&$1~4B;UCoQQf#QW=zK;1k3Ii zpZYgk*n+q6O}YtmJ`;X3GCRG$gnzz&4{=lX`18BZ-o)bsft?F$0ct;S*Z0$C2m!Uz zWDCISh()ZX={qgEzKc1h2 zU6Zs6iwqY;#ZY6~(-Y1hG7*BO;c_z?ZMn{kI?<86A6`{KiAd)Apg41Cxnu;hse_o@ z2#mTWFfNL}((z>$?&GLP<=>nzN$f55#cQ$nr2Sb(c<}X^Y?xSiktz=Rmg*Z}viAc` z!xQ`Xdse|#u%_XdNach_-~-oE-^0-YeL~0xo`0-FB|1g(N*unv$$?cCd(I;&09Wfx z90)WS4sISjmYOg(I2vb}s92vH@oo|IVC7ZX=;UoCBLST3)h|43rHzf|O?Bm`+P0cE zr@f~ozsVc8(0LI}!|c4lul)x77{HeGCJh8uUf(v&&`KCJx%Yl}-AwBCHUsfzKxw0W z`p#t7swp@!a+Z&cmnrX|T;Dr8XjsF`YUt6#17V#S59jMAk964%;1TN`4ejd=4}m40 zfRU{0)o!jKt+qqHHpG)O4R;v<$|8KY<8goAJqHx1&>RE3Gw|nd>7Ed8q4d{_orJGi zY$Od_eFzWvcX&VV#%=^a3M6ds3kREhTxMI!4q66nE6@W|DsonSfEoVK4Qm=cxFr&j zDv4)e(L{f)Bhy=Qj`C$bez{COTBGKS$UTW8zVV@KM|zk3r$}Y$m#+8ZbTH{|rl*a{ zasDD*7d5`|h#crw_)WG{Z*$L{&uxmG=X)^OV#wveej^-u< zLzy7@v`?PUIsY#GhbyG#2nD)&j^DhNeKe19L|dfWa*=u>0FpKqzgyc4#oFz|!*PuVE$NcbGZ8}p5FXNm z9G+huuieMRHCz4}HBMPPj+%x*3*6$&`O{fxKEt>HvNz8Cp|{c30Nap2I5P6`=H|wF zzDX2({6|yXoTJrjy^D64UJ{L5FJm=!%j6u@qz9ddkDyrUk zmz8=~v~f-+{uilF!Z7h^76Hk3cINZDN(r)^tTZh(fPDlR0&V(W_@Lzj4vwhOQm$vQ zbSlk0r-!Gf|G3a?EnH`Is!nj;XNOx(P7WInZ}q5$apTyc$742`$M79k#ouzn^y9Vu z(J!)SQkOsZh0J?m>5*Xj!irTi5z@)F~*W5un^(Kz#j1$YO* zVUIcqr7Ioh#PB-5K+pF@8J__1A#t%7af$qa15);WNxZcS4`bw1P)qJ&^D9@1cqY3=`SG&RUUW2;K@87?Hu|q| zkcbQdQt!2vqcINa_`UEErKGASu(C$L7w-2xuD%gu8#f>U#0MT`28d?3DOEQhjPD)i8bq7BA;XeZ zAR}L;n<&$tNO@}W@u&lrmx`I0xqCdNu{1!aicz(Z=Gi)%s^MItr*M|v+1mm!r^Y>@ z0z2TrW)g@SIP!TQV|pJ=&PlViwWU5W>o%&^jm792e=y8QOl*pz;m9$-W_WEErxV7I zA$C?)sz835KY6~!4_H}$daZmAc8%pVZm|c+91S=lZpG5qepv!mv9G?rQ(QS;2pEQ+ zx*9_TbCu$R7`zsPGFEcrBWM8i7knZh0GYlz+q0868If0)UI6ZJ?Ar|Z3$5q8cY)Rn z7>Cz33D~dT4spWr#$5=R)~8;v#!s)snJ>9QwZmgSvoZiQ7Pv>`vB6wUf4IQ;Vkvxosk^0PwW~GIXQ7PG~gb8enye1sH#5T;fcL+mp8SD zYdYT$seNB+%((PnZOK;C zH5{MTs78vI!{3pSXnn{lgukL=X*9NWr1R~wK$a+X`uk*anRo`0_ zp>+Jlx!E0%o|BT0bO4v)s>r@YMa)8mzgz!iL4sFDXXg>XG(gBj7uc^i>W?=Xe}m%5 zWWCD+PMuPdqvfuq<37$8DSSbblas!kz2MAwUu_knf@2K`?gJ*KKm^G6KhN;#LgRe> z;BR}k3ICJ$9A+bKdR%!}S@Vdbb0Wzo>li}#JogQ*B#wf#L{LOrX|5#hq(GQ*S2NYL z2CWBkOG`1o%?vU3XmBz{LH?X!r&D3HchUsB?GH#F3l3t4u7Vk`rG~t(C8xL))ots)tVo+W6=P`nwpxh2?;%Y&@nJl7p%q} z1LJq!o-b&CB&q0CE+&92(Td-Ihy&-3iizpBnW?d(1U@==eysy4LhH{A zz5s-#k!mZAXCG*~KroxCvcL_&#QzF%G0{45BzAn}*6)@^Txx&}Q0>JFyo2U*29Q3~ zG#NE|>>*2EUVAl25F)tDhvAA#P<_6^w3B9h*0DY8eezogter!954jq!nYp?Z zMqGM7BJ=WC$IA`-`*~Y4;6aQG3`5{}Oo4n~zt-NgEf7_-)esF?Lu=e`hyi7R@1U3i z?BWe-Zxxs6=PCSVNc_1tn{k?gLD<2`Yq2dvR?iNa-~{QyPV1mT;CnV@HM!>jLOgm3 zu0WY_YZjOiRI;Mx9N8r#GV1F^KxX@Mf3A^Q+Ft?`DZ_l?K;lUy=Jp6=!i#`$JmBX~ zAm`Ls#dGuheO2?8;RG6SZ@@}m7b=R9DgSf5H0!Tl1oii}n zpK?QSyiSw$U<^^i3FQ6&SRY6ejIkU{JL==CIGnbt;J2TwlrXLU8S_B4bT$A)I0!8% z;xh9>wDz&{;Ya84C47Z^&qja?129q2)Fc6w zN|YCAPoqFVWCW!7J|~;9Zf;M(yj3?hz}b-z3M7^rFVdURtg`(+Gs}nbf!Q=OxZ2%}(~`(jypr zB5NIHT(|#nfU1IF;W@U+_TM59iz9yLDvj2=m;o14)YGFu0kt%9kS2oq9x*+=8c4LM z1T68A9=`@K;eenf;TW}l<}Vy!Xb)z@qvBgkoAA3_O9o7?mFn?F60_l_5YEje^rxYl z#)Fa)ple|4NIQos=B$F2@$+lD*~lu$Mlu)hOBncs5y*54=0GT?qeEe8>JD!n1XzcS zkI%->|Jw#79ouAou(E-duFs0e?v@yo{~s;C#b9A!OPV=j^78RHt_WyGo4oT9G1nkZ zm+-FY2wELnN8OW(C@zCbwIZ~Xy{f(J0OOUN6=FhBcy_klj@6p;u1imS52T7nf&z>( zqnM-;wYY}$N;*Y=?zrj_L*zp3Xq=TrQ?7oG8#C|{&MZW+d&U(@_qJ=Vj$Nxn2OFTU z_HYo{Y$|u06?Fn5`YMKon~q1JMJ%5XwY2T9Ff`cx-J z42Q|G4c}m9A2jVktsD2K$a;>t=C1hE`#rIuvGVXOHVTe-y>pq8U*6fO(wLkFf_2eD zG1r#X#0e}bwWv#z{Jk|q^nzt2o6%(Ix$Iw-7LKJnJ!nh)v3HZpHauFfQYm9a=aXBFjbQrUqT5xCU z)D|v;ynA;B$Er?t+kf&-ERT`5MNCn_V88)QZvWQJ_AjvBM#9IBa-a$jT@wr3vdVs{ ziUoQLLN34Gl^Y2uY0NIPm%ND+lmsBo?~Km;tv5XkrKOLIZ_kh zp~Y@oS#XO`&3kXqgCd^ME^Xt*^)+DeZ_F!EfxZ3taMO;M%FEWcrcgmnlyIhK3v>y3_ z3W|YlTYth*Qm9mg?=nOo*3&;Y0IOu@;(8AXQDV*AQ`OF*6K}q|uBlhKZE5ZUAj+t{ zyS>);A>}b{c>yZyqXing&dn%*j{I<(qmh=Dp00Bm2CTw$Xw5xl2~=?gi=;bOO2hOZ%&?_o^iMq6wY;1XTvh^ZUED1y%_{pe-fb|HIPPxH-8Gti>S0_0D((UJ) zQk59}5|fR5er*XNp&C@nwo#M*oW-P#c%1Xm(z@XV~D@zOayO?0*?HR-(Ida4>A>Ij8t6+^qVGzEg7& z09Kd9wjkfu3bKSn9_N_}16YP;D`usJjpIdrtGx-& z5O=ruoqX>%vIFL=$4gU3FOf?`c@I)aP${PZbw)m93Epk+ze#z8o&{A!%eGh3%`-K(XlQr`PWtObhERYn@mX2wb&hj~=jX%T$S?^q z8^S=44*mY7Mq98k=TibFsl9*yzT4(Vdr=uM8lYnlAUj1-&vMJa#SR=8h`G495RvSe znMrT)yFLb`UzGUIpSzKq9|T@p1E*RLT33BDoBd9XI2-xzti^p!9)agKU!|W$fZF@n z{+z0ejLgt^g*L1$7_Q|p$>>BifsXS({*Q(-6bV!5mjP^+o|r(y*W4rGVb z=D$dMeSPZz(tsBea&kJzsHnE9o+(p3whV};Oq_Gv_^j{v>pnjW6ePGecEIVHsa`gfBQsGI1~VV7!Dl~D2hKaZjrzuWB?O99=4+qb{q)Cz|EKj$sSil zwa=+-LG6FYWM>9`%H2g1`amENiK_qy2M0Lq;UFBZX4oyXNP;ozB^52p1=;>zofcXW zLBMT0rO=U)K`}Ko<yMBa68(ejExg0V#x~iV{EqBW{QUW|(rH0T$YJ^?vJ4D( z915}vDek$ydbyEp1+q>+`^BveP0yUxk!}hI?h4@6?4Z2<@ZrPo^Lc0-0N8uZTO@h; zqAgBi#m*qUX_e^-j+ee&(f$Nj2q0}Q(uF>N`Z^!LIk~ZGDn05?EOr_N@gOKBF zFS3xcQG?u_M~@;wzoN=^>@mRUw=h>Wv+nmxhfAHJNNyAj5^#!!2)S~tn%I#W^1|8U zVab3VT4(;X>4~VwM47O(uEXJgyHC1C-eD8Q0htfg(unBk2jisgltK9fr4aOB7kuIP zfcWQw{wP2$K`RP*k2?Pnow5{3s1RTrUqRxI%sHCQrtLuS3onrKfK|H;Co3xp2{Xgx zSI7<`XfWE26355EX0s~}TzU}53W zS)GwYfMSCs$j?D1s8D8towZHZSTvRqI$)r3H2K{O>~|tKIolwqR2ZXZ1#kgJ=2z}J zI!#SDzCB9OpyWm(9kFnSYJ-vPYC9=WIoFKwSeSEp85C0|@Qpt^gMJL?5myBu{G|{# zkUKEj6L7%qNh4-OH&*(R1`(K8Sa5(X&WDTbw7R9V0Ef*0?BihVw}F^ULvX$yJbnsxy{;ePXPnbMtq3 znNnzfYv+(kJm2rx9~^}2Ptre)Igkp`Bh z&rPhjuYcfLD9Ib1Axuk3%n>#Fn28E=bsDOIDm(kh3HygPp?=()`8a*-ZCvX!RHI)% zDy}G8Tn?8a;=i(yDCD6-N&@4v2m zY;-BXNBJ{KZw!To)UwBa9I^~acM3mrhEvGzhb2CFVsh#FrjJEg0t zwZopC^)#6y3}gE*e>OXdrU4m|g2B&z-051uPC5Hh*r50Rm-r-$*te4n}5&ck`XXGZ6zR#kPR`=k5hK(8+h8;HeKUhWt= z!#x(wE0B+mkSTGL*bDVU+`%A~Hl+5m)`o95IQ#L4pl5A2moJ+eE;G^{gGXM5*%QO0 z98dW(pzKN-8WUs8%RG-?2M=ZDXt6^yA{P8ZLeY1=zC-DWOOG2X8Elo#$swY^bl*v< zW+$Uk$m0X)iC6s?`U~QT<3m0t_P-T`=d)(>?6XeiGnA@X(8xNL6Js(&2#s z%OERz1%1eJiS%h`f%t7F+ecvyjUI07&cIwH({o1wE;`AybI7g-gfVi}9Ab}_A`k|) z&|kag>sBJmJ6Gh#_&(nykidk}2Y){p$&Z;zcU}zi7GG{d74hIgY*?D7Ot7(uPW$?n z=HzIy-TIMnsJGtP-c48vIFXMRuce8BoqC>tL(as~;Ts5h4-Z0fUX5IFsmae69W_Px z!xILF3FYK6H{J^lyE`I=`TP5iElA!<}fwuLT$r5-jyC zzfqIKGPp~`GI}zQy5;|P+C$cFe|Kl;Y2ECzo~tK_;g`_S@Pw}Z_#j>%BlLH!p<$5n z)XLi0CPuHvWtFJ^(&j7BD)F$cpLP-|DyoDOT%uGb=NL&s))hlAw@rySurbhyUF_!P zEuCbn3v>65EtFMU_H9JGpCCr!KUB>KbJw7iFj<=ZSLPKe!q zJ|*T|defTggjn0l=UAS!62?&_NEy1HHemK}+VwM#@w)mAKiCvC|E!63pOr6Z4sY4Yms`_q6 zXR?+X;ez|RSSEsCJ@Lif;bV*TqZK}j=#~0_0EkHi=Od=bNY$ymbt97R--lxO*C>cg zFvG^n$jS2D#C@rx?j~I%%p*l#Pm-VLswo)sb)wM-3o){5q@}Ak%x*TZo?OB!otaCG zop>Z?N>jh7Jl}HL`UrQqJT5On`y0Z<;<&l2I9b^~`{QB7TOwv=B6@sN(=MYl<%H=L z`j)$Cmz$rYL{yt;dQWeZ6lP;9coh`eM>RS#J@pEtAF_k40H2lm-3q0`^RFjv8)mv^ zOq&!Tk@aP+YhV1J)?+y1ms4E zv2;O%QhZRF@>lT?vB#8LnTk5>!*`e%mL!6P>Uk1p$$I^zdjH?ck69NTiu^q=Z=_#4=EaK_0@=j zUn@kX3|j7QjkLB>Q&rJRl~NNdOSvLMPtHB>s<&}KIMTEP06ywkeB(t*D64Pb@F5zMLmvRuVgjv#}Tl3mkV2NU@Hf zyxcy1c2$8agXDHYL*8!H%+joA&v#&*$e4@k&XJs#jkURZ%ZdW5bYiPufW4R}K~)u9 zPeT_I%d(u5)Ozr9Q`6X5)#OF^@ZesqVsw9!nMv_$h#EbNgDFxuRi~v1HZ`gyEIR$< z$w2%U{`;B+;Hb&UmKQw`M!o{PsdHV{gO93=f`MD9C9xJO7_ zT&}~6(3^(8?t&^?pTwE8EB3F{(*8|jpm@4HQo-ouBQ`x=8iY@4p!+G2A$*CB2*fEv zZe2|oB4Sv0CEw=U3~a9;+hjp1Iq^Y$?^N|V$-)%U5uT@fQ)3pJcCn{AJe_bp&fBs5 z`0=1`u>IffzO>$8IKsMIGO`Ga{kLvtzbKoo&QlGHk_Lwu>h>og$Kdo@hV0K}>gr4Q z(O3?TpD0~IK7OP#o+z~`-`wK)H==!%l);y%SjYVb+ru<7lBc(?a2`m`85lp%pKB;D zVdF*KiU?cXu;Te;(9)F4&7&?ro97Iqas){*=(pJ*VC&xOmt{03dv^04y|vEFyYqVu zX?f%F&e}u)%JS3^Pk33SV zVzWM+KWtV+hI;vgJ^t*+xe8M9ifOpG)g57d0Y2?&XfITM?PxvH_S@sGN%WTgoWPF- zwzqH~akuy_-9T5>AGx6NPZ3xrTPqR~l5AZ~b0&3-_=zuOUWBqnZD3B51rU2V&LZa<(6bMc7cv>Vj6FF3MX$N4x~4Py|!t zMJgz!$}$52fLtwBXd<_`hX|vv&2pL;j@-ZBn;+bzT-W_Pal2SREDlgljIot}x;~Q| zE8eeZhW~Sc=y+g{{ksKi;Q5Be6HyOZSm0$&2*%mTmQanH!$F zH$WX3<&w4kFn{JV_fXGZ8thwaFw#vqFr}pUyedU)C1;AXEH!{+t-M4vI>NMVyexKQ z(PX1-GZtz6Xq7M=KJ!TUkJ^=Vlan+}knYs=h^Y<8uH$5(Eefy2)9sg`)@JNIz93M?uT4^TXLE79)JwTqV zhQ(s_>x6ok64BZ6UJxhz;S^_GZQ;$#un!OY@iaM!O}MS1RjlRgBsAU0!;m}d-%DW@ z>k0#K4T|C5;4=%?G2kj9U=%74wzQj{^&W_7)Iz=!sjRG}a^aibzde@GeXojOg&z~S zN_52;>zoeK#O}C|@Fz?{h20sE9x_#xDQOoTx@xZbnk~%zMr3z(OT=+@a+mF~3er-u zFOnYfy$gj`4cOSM?}g{FB8PpqO@bde)PeomLEdj#4u7lz%p)gH?~u%sy2A=QwV`2k zT2pheZh@}nbXyb=z{T+t&XA;G3$Smm$gq2W_a3y%75kG_sjumESM7196hWQQB zTwK}J`%VeEM+tQD$?Jojtz+w)`>6phSa1SKTPIqBXN2Qi#*F7~U*llGvW4iM+;vH5@4mVtn*!OF7Y#ypzS!N)l$3ewBi)JMOqd;Es! zu8Bjs@$RQBEz(FMcAGq(X_ILeuQvENqW_x_WOiN`rjXMv`(&Q>fmD!EV)9ZH$VX&Y zC81-;HhO?XJVTylj+|ocpo9lG{(c*0YyE>o@xX1rl~=JoKx?@j7mas6aC98d{(}r0 zi6$*AeAe-&!sD6ZfCsQ^G=OpMwlqcjrd?*M@&zlCwEw+dMg*3ElKjE^Pza?)Mj)Yd zsKbnbuD(refVBPOqDc80;{BEx_tRNxq0W)a$H4JeM5!$YgCQFkGx{}8q??+W4YhP_ zAa@ck($^)L-40DQUh}GoA=jk&kQ=|{5|e~9bEZUBxIlS*9{o8EFK532ba_!$Nk3i4 zd04W%r05ZHZxZD^8ezGM(d0vDn;;y^x+Ns6H{fV`#Za_2xW}%<6G1Ai_h-cOkwCA^ z@j21_>|x$Fu{vQ#JV_ssihQPtS{qVdMDxmI4;F*p-;7t9)Bm38YGcFa$+J7NF>90) zOie-~?4f4JHh6ct?lI%hm|F%&wEVX}%1G?N3`ukCdRJxUZ_7X(ygvHrMX2hXLOAxk`ilSTHC7NZWPGxpcuXAg4r{_X(H4Xa)hyr^ekL0Il;z z<~mc;pV_3WO3A|haa)g_kYB))U$r;mACxT4zbV=O&SC$V4 zvYpD^zunCjNi1{9}Pu!GJ}W=RqxK=sm3ya$H~t4vucErDX#NzZ^Ir5}2Kxopb>} z<~v0jkbsPa3gz2vsBpBCwxFhAVOw;1v6S;I)jtti?IG*jVVo18hvyMk41qqU23pCD zNVrX~dx8fsM+{4MCOq&qCZ)qG(@!@e_w|jgYv~a2{$udU_1a?x_UyxA%k>W>45Ys( zGgD#iPMN944A^y}rG#9j2?>VCHTzh4Hj68(@OzrhHjFjMB*qcC?O+16T&Yp~*F2=D z0{TE2=)uj(`bOHh`tnj%A@s6PtKfptsIM2a5tw**5c;ejffjjL5hQ@L6^zGfNXEBG zRPwNID*@&WO_#)3S|XybByi-4o(ES#24Id3E!cEQsWuqbk(ZaW*204{_g4lRZ8CzN~cGiXQAkWuR1=jaMKs~}&!PWIn zlEwrRyTYUm#FX(M7biXu@3A%kE1{3vik9zBI2+jiJsjNRIw3E#;kdQhu?5wzDhDv>F`T!~2D#d6edJ)FRc=2Anx%Fp~=4-x3N0qp26 zhe*Kuu6w=L?;9tb&(vIaoi}agNBo}JC+EV+&~nW#et%glGABCZQi;!TRs@vJ6@gR;c?S%L ze4KrT(gf|mt&$F)=LB64WW64UJdaDcIXLbICUNSz?v&MOw)lDjp`+JQ7-jFkfLrm3 zQ@}Otuml`WR#w(+XZ!<@I3T4IVtxleCj;)sw1e)6kNm>K@>ir=DlY$DWql|s-5*wf zgbk_R2WyB=OM4F59d8@lIMRh2Sj5EAzy%6tzEa7&b~EM$Ll%g zP6;k?pqc=2jT^Xq1De29Kr9I~Luzphl0Z+>S6BN$DR!h8>WLJG0;x=jnEPX-3=wfP z(VEfX2^uY+bqVx*;e=8bfuPTs0{s1BMn(o?5Oj@@*HnNqkSo};j*1F&mJ~rxsSk)D zUJM=m>kVFjHf{OkEKu^V%y}*h!fQb1%noeD3b<&dKD;$uQ*s3E!O(y{B)D53EatZP za&a23lkX0QrD*@FMW{Xl`aMpdn@tN8`J%%y@81tp&m(R&q(yr`tK{_{8$TK{g!l5H=t&LZg$! z4oirSmjP;^$~8TgBG4}cGC9|a!zJGxEH5C~NtN;w1GPy;E1)d{&6Te&7Zx(D?6j@j4E9G#r7N=Y$jXlN8bz=bpv&?uy6 z;OXSc=#cfGUHt2Baf>1!jtBh3$OK&NDb=rw1)63tASl?RnGO4Qc2ix~`Vx|o+y{4% zBArUp&U(b%H7c(DOkGTalt5px1F(@H+?MtE-Gu5`0=AbD-1%8jCPwGaXz9IRXN=jZ#>=E{MEi z?|=*mD2Rf<88`-)vG`qAm5ux_*Y4dmA(dZ1oCB=67-U`<{Ilt`e8hv`UqMY9$SCdKyP)j#ZMeuY;kuN^{$%Pqm%SW z1o|kgB26}=l0i4wZSJWuxJWZQjl9?s&n&F|R|W)9tYb;Gz;xAdp#a zS$h`fgG;L5KOp$8Z%FX?pF8;fzuVvlf$I6}(!RJkuMhmM7bq%<>I&s@Cc*y;)1)Cz literal 17543 zcmeIa|NqPx4hLqI?pR6szwyGt4b)}k90Qj3sW z#F=aV-t&HW&p+^9*ZHuw7kfWG&zxh7ImUgDdrTr#-pf41qsBu+LwhJEE2WBthJk~I zh8~K81^!E_(1Q!`hoFnJrprfrGZ%M5CsQ;fLl*}ddlwr^BYHPeCud80J6;Yx4(^xq z7A`Ih&O)4=w*T(~9QID;oHK^Ut>7iN4zgO#XlR6nsDIE4#R@FZ&?NuKNr|g@WbQAz z>n6M2^&i0ugXtL$BDbWZd1S1@%-(64X{jG1FD}*%LCaqvnwp#K5iJ>JI+aEF$)=4m zWu(|vQEMHsItmUL%gb~c4g&h$;yVO8ke42rGH`9Uk3bQ8toV$;nk&*FBa7yz7 zB4T3VvD^UE&#&=tz`u?D?+^di2LGQpSYJQD7>tSGjjzLgzaLmFLoYJzWs;2c%tk?+z$1(P{Y9n-UO0K@#3u)%Cp5|dlgJ0RLIj*Rzc3jd ze)AzIoT2WTm$>YMauX9LFs^a+QMyxnSfj%1SMScYr#G!5!+FsaH(f6rQXg!!_|5C3 zngu$6%tUKWL`0Gx9!|x6s7bb(1}_uHZM!fF<{WIyN|Xzxvjb?iX>mWd9JF%!|#{z41G zs%2J&4XtY=cU}uLlHJZ6!-NFQ94|CW@swmFJTlsEg%}xC&0f&!DqgW%>@&?=i~SbP zX}9(n6t8{!Oy8gmf+Hd} zdLl#_B0unobb{v3dHF0L}NR>aAlM&;mvyO)jM+#XmRe&TPd z$;S3#YP;+kNC~r~L{37^_r9J6VmfpBW&1ZwD=r_RA3o&Bo;zk+r`bcI*ZEtF48$*A zQg)SxvGu=AelM+^4LpR8hokkmG!$YN{IU5&w14$sENu{ZZYO6=11oY@HPLX_W9F>Q zRe@1#XTkJ-sFQ1S-D_gvBMUqngPWhN7X;R%;`&|l;>~g2Bu>_fiz_nkFzb9xMiw2c z#`4F`2?Cg(G8|iuSu=phG5Q`5e@%5Q^bA3aPC!@OOMh&6$hxYaVUiubAy=`QA6=hh zf?I-Hx*RxHPv&XK$hf98==OHzjG?536R|E{yOM7IO|^NgxJbYxGjyM>vrnUBH8DkX z*UjRl&ijo~RD^6LYcskbvcF<&sU=S1FFs7M?yDY0=^tguKX0HBu?nXj4sipwHNJP= ztZeEDpGHPR%qNYY*7TB2He2L+_hHS|8JDZ;A>!Z0o{Ac8v$UMI2 zT;P7Zd?9*DV@f_N(j|TM#K3xrm@8`CImMl`(64t+%7J!H2fFBOzl-$<7hzhvO1g?` z)?DY>vWr^(_#z`@6WFHF8H!KopH9l(qe>%c$24b?NFH@Kecs67z0l}vPV2SJK{1RozEe3r*L$V^Dxz`#IASlIDuINjEzbEbe}5b~fo?$@W{M?^$#Pqs!bkNWA* zDMh?qO-xL1z}t`d9!p6{nb;H33a8H7HOFc|e3Pm9ti}sJJk9n!(<*DbvYPjnmXop)l>ZGcY~sOHPN>_Q&~C}q5!X!Ujv3Jxv;8^*-KU-_Nz)YI=ZNGq@8 zZQpYBaqZk?_aTFFxSGirUSO1EVJ+Qyw!PEUrAcP}WVAV$Ec|a=^7;nhU*L{da@iga zr*t#$Db9Jlg#=atM_jaceioiXEY)rq7HbD|9kMR zW#98BR8&;>lw1KEdQDtD7l*pmzlVo6erDJi%~qNEoXwis%+*Bt-yxShyeVurf;>;Q z5>iutX9_xBfZhCsmD91Qmlz<=_GjTX3k?YmAHN#s-0$2>Z9HUp;@(VT4-+I2R6s}QF!2*2wtbYzM4l|?*o)-OTtvyvgt z4<9~k<)`QmF8Ki?0dLwKmoSNlEQW-Hq|*IV=4ZBS;f7p2?(jiU_W2z6;O~U|^(5;X z-ZdLe%k85yruEz)CLtk7ywT?7=4KWWn%g{;iKRBRYgolf@%292<9?eh)*cZ-8ml@R zWDsLG_3?aOo3P5mTac2^?9tXEOo1Oieq_Qtyh&(j-$#?PSDk+<)g}v&3@0!}cq3ZR zEf$;jlG!wv1q4zF#jfKQoAqEK9&3@$EfyNM?b~l&lQ1iO1Ci(njgOCiTdG}aF;zGr zZQYtg&aS-zhr_GNA?ruklOrp%UNYl*>b&X<*Q-^JoQe8}|bRnpXynj2yk z^C2R9hK$0064Tj0{jorsD6G3wr`~N=ztxNH0r^XM0RaKzM4lW8TFO@fkh8q}Xc1uV|LmtH_N`f?0Z9dkvy?b%hG@7^VAfS6DcuT{XmS)*Hk;L-RW3RHGQpn{6 zc!rsQA&@~bY({A1iI7VKyH4H8{{9HGREJYr(!gLvI?mhEb2e!7@&s-R2YVl_E4N>6 zd&b06X*NRrm0hPVTg1Bop6y)~hdS%i#TqAlkL@dLW_cbxdW1tr=zc3~$XqLedRf z{ki1dzklmOcv)GaTD{M|Bqb$Tvq^xnE$Fa<)|a}M^OS*s0dfij=h(Wu_3N0}UFKwg zvh&R;%)CCzgt%ND642ySa@GkN7f^aI)b3!DCgCRuW&KC@WfJd<%1 zHhU2Ut^FcNo8w-rUYaW8$_g05#YIw+SUP}3@tY8)Zn?pFZ{6DJrOH=K={}uo=;YvX8}V6%(6AnaRfQgv+u< zke+Eoyplc^yq^tY_B$SUP9x@*$yVO-1sxOX@UWZkV}8``&R669&!(f9f>ULB8EoZr z1pH;pb=ETP7O|9lac$pq4)AhSOy*GRUE^=L2P-0N<3Wk4aQUkW>S07|Y(7ZJ!pFa# zg0i=^`RUhfVN`@iiz?srammq5gJTcH{WiO;;k2H3q(_b(z_T3Q-*eidt08cBaYm5vv)MWU_A1PrX7 z1wf=DE{R#|IhUHGq&B|DD>d&o*5=Oo));``$vy$uYeKDE`g^DE_@((Fdk*r*x!>US zv_kC8msv_G&m-wGd5BnDSIo~n!7rVwVjKd{Rc9Pqr-`9rwIT_Si=xWfst$VIX}VNT z%W?A)&^355PVJ^39YOkV%YRD$vJu-&xegJxP54BV{no4J;Gg)#rn*2A55igdhA>@N z-3J9YvgQ5W3LHJYp6Y5_9-^?D#pv|p{PeQ-<;|;?yAQnEhme`S%}b{Frs<@ldP?Q} z=t7k427LX4kz@qIvC8ziCw#h&_DgV^EB>Bk!mURnS`h%lu+FmM6unsu$2!wLUJ7)u z=DDXna?0b#Yiw}yV&+1A2|o4s-o7YSc~Ge~3nOD_u1P)pv$n5Dq9(R5HX09}a7^u6 zk}@w|g!7km1J66%=V}0r{m^jIAsVF#L+rloqk`Rl-MYR>DqhcS$}@2DLm$EplpDM8 z*w-IlRX98wx@iDm9QkEFsN%-hTL7&P23h^~EOPq+_hjdg*!jBm&(1ad#=CZMz`Z{5 zU`m)_*?4oF8!P3oE@^KZPdDHSja6KI-Y~nvzu0^odJjNBH6F~aTdGForjH`z5*g(F zB2JgTTthi{JD&@jud`GeINDohVFSB6G4awriyiXy@5YX*Ct%V2R2IrneUhE-c^6kv zLTIuz0TGvy!c!V}fKxQ!U{L4l-`J#*>wN@!8d~h+-C#$15xVK;GxkbTw4-U{86%_g z&yD*b8^S-|+g-TFrg5@&=}5vZJ;OuvAe~W$>56hvc{s@>0S5Tbl&cW#7jJLA_d?=G zvnnjMdDsy14+k8H?%a3BLVzurzK{_SaE4CppWX1;hXo9xo+~|k4Rx9j-EC|ZDpGk)?e2!ni$I`!$5D{<$oIKw{?k-_2dHOiYwz(cGq{-0v)~G;u z85x84mppPEma6AY$1T%qW8Cf3m$1Ft*eoK^Ug|awg?D#Gl03itbDCnj))T2#sPg?S zY;U2FB)nT(LIPuD=}J29AFCj`;n>u|Ijpt7V>e!c*LLoezTf5MHAJQQjmrg%!%SS7 zqNW!ijjo!-X)-COg7i#Gq2uG@3faY642+kbcrYZ)g!XPm+r+-%sqJX)ZOX0|YF09W zDssQM@V&i7V!xDDn)zyq&g#tE5NFxFqmz?d?OH1Vn#Oz2i4)ojP40zsN3ZS~B5$?p ztq+13ovtq&u?T2nH8m;4#l;s~CY5X!o2EB2i#`_oDbeD+zPSO^$Y5`#lEQZcmxP&! zntBi*Lo_6NSVRQQ?P<9`x68JQgoH%l#{vR?=>vzXU+UDwY%d{5xO&2&W8Ln>9TS9) z+-^O)F2xL1l{h{+`U5akW4D+NU_Zd@@0Qzq1zon|OWQ!bo@{cp4hjkaDIF6)G)KYP zZbC7K=}#KO0N8_Ka@j9-EAG|gtlobIP;rgLM3B?w;D=^6+jlXP;#O9_hO8Ac1rmf@ zcZf(yd-m&>M<;S+IIq&sOLQB*pwe+b0Gf8a-N;;R8TxjYdZ1pj8+d>a5Rw-fWi%*u zQIO+j1Ru?E`TOU7Kt%-?4FNX5q*w+ZwW4q*aA@G+p@UuHCV8E``)A6*rc!R;z9yHy z5-KW00IZl?9&d8gqVR2MDkazl&VvU}U%&pk-XDhv03N80Q{|AHj*hngbPn(@`#2Be zK%^FOfk#>=otGiV&ugW*_*B9kT%dp(fhzYB(ne#952`=>`tl^;)YJ=rW5+R(^9sXm ztaov=BZVJwQw1E2%F55@tjoEbHe^wC9FUh|pXEy9elvgE&y?Ij?Z8wvOD$9ZiO-e) zfZb}>SahcI;kI1M5@z4Brcekv6Ok|B!{4Jh94rWeNHhg#tKWUuSH{W7>5RhoJ@_6C zs&WC0esOWZ&WIwaWQogD#cvV!1`f^ zeI}XC?xy+g6cuAUHxguGxcdMOrRo6Ae}%kAcq*f4VDK%C$25w&FSX4VnQph()Y#IO zs4Qv;@I-a}F0|BI-~Yz-WSDm>b!WOb<8&7Wzs%NxpCgvVyx;KhPO25D_QueOEm0Z| zgViW`cr*z5Uj55?ZjwrvV`q$3eMYwD|8C2TkNKu#-(0WJDIa}!An|!Dci&6RiVifh zgN4RfIEc7zv{}S-(Z}Z}<^DHAo35fa|2}?f^*R+6n*=m5_3-d;Z(r2+D)q5_%fx0T z05|ghVQgJeBiirUiZm;K$SEicE%EzZd+eI)H$(GpK`1{qjTxI-88HIVSYr+2@P>GJ2*f_- z|L>)lpP%R<{ayBE5&&Q~yX(VIkM2MP;uj_bi-{k-Ov`UrSy_>d-K*a~_M5H8th1Sw zP*WpCM@RS1UXh+}?uXwPVZ+6*Z%Xv4!vJqkt+&$yX>5C;kr%Dm9Wi>+TC7n{5P!1- z{y>pHMikBA_PKBXbn`#<4XDpIoqD^e60Jm#$v_JI4U!4UUbn+@_4bJ4YzjNb3xIV& zm_8yUO+r|sj?3-knE#(jV;Oy)BWY+k#DC%hz0Q95J;;GYkap3{p_Br^s@`^r=9Z9$ zETCvPuleYBsSbb2n8TkuIgWZBsUBw!pSP*6kCvr`G4%8U+ty3&-U(w&LN%P+E%Uz) z7@J8LE4s-vWR7vV=@q=s0_4MBFp1^T%N_!Ojl&%Tsxlg#eU&-scI9Ndbs?O#llSZz z+DJM(J5h~*p8oz5kT)@)rQt10Z+3mFmOv;b`d_cpJ$v@dMC`TAOb190xHvclurfUt z#8MVGZ@jKMYU<_sBMpw~=7S~kJf6N~6ek*Gjps|=D2NW)9tPcE4^aspoJ!;m*uU+G z;9QwFRQIOJs23091$5FqUOfC^W1eD~|3Y??ccI2I@rohB<3$_+&g4&}_0jMpa|Ac+ z2Kl)&i!jD`uEz3!r6egjCgz!lhzL>vV`Y0H7aYHd8q3K&=dUld)|?hWlmW&|ytFj> zIoEc+&J^VE>s`p*8Y&-}PEP~A3hX~NTWwx_4gl$(*Pkm}oD#!kGiN*+LBA;fI13qI zJquv;vL~dR`Jal_ZDy-N0biP-)z{ZA0TA}zKbOAGPll#7AWDEZ&9wWs4_QmSdv^@T zPSakq15zR(A)!R4{xhI+a3poWT&=b9-OV|w-e-HP;fh`#N_J2mp*7STqo_kqk5o3u zJxy-=@?gK!Z^+4|K(K;d*3b$3@ci8T{9v(etQ_k=4WDN<^;M&sAf=!R<#Ep#b^%qJ_V2R zc08ZiOcz>Rfn(a=A-Ua3GEqnQ!Rzy`98?pESt-2-_yd(dBO@afvxU<@CyF+Jt*jvk z*lfuUY3sH((+NuLAZTO2kEucTf(wMX3CLE!hexAt5M0h%!wR> zrgy`ho*=WN@mZ6D1|V2b7$`30*v(C@yK4>ts9u$z&jnXeQBg=}C^0SVC^nr>N1g3_ zs+hlMoY<{LpjfVXj_+4cBmi3IygJ>T1^f@>GdO6*r$v}L4*S$3uBJXdu!~y^V2}? zmVR?dWjqYR>!HDy#Kh|Jkxao!&@Ak{hlQ1Xyg8HsOps0}dO?r8xVt?ULnZ&cF>4Ul zb+DU@^*ALE{HRg|+8^Pbz{4DKaubxspqpj~K#Jw{^|hjlOHCSpRMNZthn?6XWE#4L z023;KJ&b^z4}(fq1wnxP$O8ZZFF|-WKJ}~Lpk@J#s0boCw!OVQGAfGa0R_jafXlOa z`&jPL_fc91%NXwdIJ)|qC%O#|-~Wz(4?a3S=O<%T;{qm}bu~3JO9%L95s@2 z!1gm{W;CloVvoMPW?Z8sFcx-ZX#v3C3#hI7$1T86LBJHGlG1CDqIcKMVfa*CpoAT? zV7Hx$ShH*|guDuxft2zIOG!Ob7Cqpg=hPC(d$23a@&p8{6hM|mTGh;;Ca?c3$k{4u zK5*KnhaLLKE}};^{__+_yBCcJ80GDpFTcQ}AZ9TXuDj3h^eK>bn)Vi(IbrJr;lZHB zBxGjp{b6}8U(apacX-f@a9Zg=|D{nrYFoe92f*hV4?a-^)J*fwrw~d=Gd96LB!R94FzhXfpEeYpufq@-9jZQN1Xxm?MW3pE# zF+vu1wKA6y?MJS+BTR2urOihv<)+qZT2&xk;RH7*^)Hbc+xa@~SchF~Ck639tV})q za4P!IQG8ljXPa+y<3SUt6}03%$Zsw>;>U;!p;YIGc?ENEp4e|;(kex^M=&PFwN7Cn z`j$JUe0;55rEXb%V{^)DI8Du)MSXU*3h zTQHFiq@?pp7V|raxoy$EZ0gl!72?m%_%1CK71NIu5{%6Nm#^mAtKN%zk{_lhO`%iD2_YeH~x< zOTr^$sZ<#;B;KN_6{5lPz1bn4rhaej?+Sh3C_n>L0md>WeyzXs9s6W^+vc3j=s{+&?tI%cqFv? zfzChZEIzhjyOSek)9A;;r6{>gWfvq+zg=`??z5%qKt%R_1Y&9i+4ek?vKutq3IT8k z3=FKXnf+ql=9pz+2AIPuo0)jPGf>P#S2wei%7167=#@1T-{0RKNW_$H+)9S5GX>D$ z&i}|xPTq6@d8Y9cs#AS_`ggJ)n(lCW?FoA4Jz!hMdYl~qo?3!lGhl}(4FW}T=!W*m zyPxOye9`jUAP}sBupiY|tv4Ub2DEo_GgHUDH5r^zo-tTF@M z1kZAJnzyY_h>!1BzCGj5_C63yfoXycwELnf1pxT~Puds&Xc0rr7X>)DS&s8|kWTBs zfC3I4-k|;@?KjZvR?HF2no@&r@K>9SFtW49zBy=k3OP0NJZOXile`41tE<01{JUiM z)+H5y-r3l+9sww22`C<1zE>{PKpWU?1gGYQLJG${Z0r+IMk`DP9szx)5ok<;?gzSO zo+zjR3h>@sE!)_6GNWqk1JG!SALFAsyTifWqOg=>WTa(i-<2r%`7<5R(Q43~PPqa2 zh!=o+?mo(sf0N1?phP#*_=8pHLVqFV>&yQ_OEZQ9L8~$^GA3qtNxx+#l>*`-+85)w z2ngl!W;#$7)8vy`)8rDFO%c131#y1?X>8lot1t*e9|q2?jvGTUfh|y_fi8iqH!r<KKGBy`4KxH>+aqQIzFt+>^PuBx&~UqK>mtdSy|cZ&y!0;RZ}#; z=D&kF&H<~n-4zP81(^QH8#k9(7=Xfcc8l`w-eIB1d=`ic4SDNzWeG@vZrRc4X+G$y zq7(|Wy!`wlpiE2x$-)Pg!Gr?#p3vioa1NWk3+LtA8|2@)+TC8U60uVK)^y+j;zy6Z z+}wgD^9EQ82cHT1Cm(PCd|Jy$1H;b6i|;lO3+uQQC)JGXGEzN@KF8ihTRGL_BdOfM z>3n(=%E#yR2dmV^RT|pEMjWZ39Aa=9ksTjeS_1f(#u^}Fb?E>-$S5k}hZ8(80v+9Z z_wG4ND)DbNFTDi)`Ps2*tD)f<<$;V->1+tl=yYADKg}{Q7l0~eE+*!-muj0snwvOp z08gnXMw7IFiD0{W={%5#j7XBWB)|cv@dAmg_CB`*;+T_20r2TH2nr6Qda0A16+aM= z0by;}0|upvrRE#d5BNKVQ0TzJnJ$J51Y%+f_+1Ma8XC^_15x7b{#0*!z*T@Ku0H0+e99&#O&;u7fn=uAZsnQqe1})P=HOM2P zqcKx5-?5^chTXik?Y9k*#=83o-NnMLjdv-Q&9B3L9}!OAcijB zP5$RkCPJr@Q5WlyZKhJc9xz{c%&eX;ieUu)zFpeZ9jw|q{gHD1{-Wm4#S1g7YFMRo>bV>+y6g+!5c z&d+C{GR&OXG7vyO_e2Wxoosy{b9_QTxGl!%mps-VF8QA80m$CRUcb-<=$5kx|BDw9 z5WmY%0K>+MH8|6O4gp#zpdV195Z#pm@f$c??Y?tTI;Ie$Q8^)Ruzcu}iNE=nV(;qT zE6qEJi795rH8bh9g%bcnx&Y?x2Xyz?)z64g5v1w1J3njYUeM-3b^8F1Qv#eJx^Qld z-?$Gy+vivgu%x&+gh& zptrWZj$(Z|epf809v#&y3zfF=yBl}dJ|0=g9SF5#fW=cmdZN&;q@@Q*81xih(0E{Q z(lr?2EL7dsZpTEQ=J1T{HZg*VYT@@rMv#_E*o&hi!c-8-{hy!Y0&NB*W&<$)dKnT> zZPt0b33c}O_c)vFDtVuW_D5O#k3eyGXQMHN$f;tu);*~O6~$2So>s>8UKN#(KRbRZ zK~Gpe8T8x@_xP-)lMtDT5zkseFmfa%3 zL%I$DA3cKp5RtLC<)6pvab?l5Ai$(($%tYCX{p{Pt@qD~o6@ zI7m*0C8VGarRX)0-_0~$8<72CW$uS$^1i@GVuNQ3N#S)7&!w`~`}Z!9ThBTl2sNou z-`6yB-%^Z-cGiOc-R7CfTOyWcwc1i2Z?fqGXs-BuXbP&&5evBM{oQ}^Bgq(7?pmWJQ!DXhJ8jxIsS?M!H_nS&-Cg~+DB^-+pBuk9@AQ&C@pGeCI!AotE zQUg}THA`VD^Z;J-%rReI@gWjfIk-bdvW7%37L+|KlY}Mf4fX@_jwrJ6B9a=r*dbe7 zVq*8>Lh_KzcDb8Ip&HpDqDL65UY!+&D!F01l^+XA_+*#7hN#Is4>G;2NQnpJ(8cp8 zr^_C8hwIco;mDM~-jK7_+}JJeZt*Nj3kOpXITD;+Hky?Mzjckr1IW0p5(IxFIVRMz=zbmd@mOu!T zS;dRh$E)(Uu!$4aRzD1h_#fl$E!m3LkE30kyW5K}_Ogg9;$aSb?M!&un6til|8z(E zWNSPx^4(R0?8Fh+);|}x^Xb;ts=oeyE2f5H`I#m%_0p=_4Bjqtv#zs!=z2e0sV6x# zUlr~5?+3+tQu1_h4{@m1UYI6#uU`KFr%63qa9bxHP$Jo zznj{uJ{4+BUKXm!`Hx9Gvj^wEsm!Iozd_0gzQ5mc-w=OqnNUtnU2~>g?A)4%x73wh zRkft;p+PL^uMgd)#NhG< zupY~*9-VKTY~-Zxprw|OJU#!ACvbCBd+d7Q0Z}1(WosqU z(KPMwuaNiU-lDau&G76UT%zE;V7R&&i^k~EAEHO9HDb}STB|J8B%W$-aqyh{xYFf5 zX_N%V5hKBUTmdYfv zTImrDjD*%B7X&T8VFX2NM3nvgXHY= ztb^%s;!-aK%}KG&-%}TYsbv4OoSa%x*DN7u9k=@P-y0^@&w56)8pp0Yt_7}-qfbEu z;Nh`rYu?054f52|N5~d!IB2qCfFqLeT3x2U)!T50yx6{LBKOR?N}GIZxKh2yW*%BA z79y+lgZv&b3rR~$COiIOla?>}Yx}ydwW2P+PZ$2V$jVmG@>xqXyQv$Dm;6%wsyh?3 zG8(YL)%Wq8Zj`K+VGN(V(T&$k1p~tGfuZ4eWiH>dk8CaSS`OPpEyyzGW@Wl!t1D8ifyqGzPt{%5J#_(!{j zx&8yubg`t(ODQQ*Z%amLtMzvmSJv<|NkVat25+km=XAtJ0)c@8E4-OL{Z*>Oii^dFxD6Fm9$iA}Q(|0`#`nagmMVeqv7cLfS- zkGyxnvU2XL>|p_D=Tqob;zS9L=;=FlyuoOKy5{e{ds@Z*T_7W`b{`Ze=T&Ujh#wv~ z+85~F2X^L`kXV80tS)(%`r!!f&toIn?Q37@+AzrzD}UvsZ#X%_0LGHVRVyaPrz2Qw zZhqbQv!Hu#R{btyy?;?X`!~dGC+Wn~;2UxW4fVS3EiEN}*iYZR4aO?#cSF=^lRP3> z{Fw0@lAONdb8x`)hm1%nPl^pfdv#iEC$eGlMZ_74Svf4nuWHPmbFIh;D`dssd>S1j zSyem%8g+PY``uz=mC4s+wo-za=kFWxUnO9CcuPdgf7FM0&A;Ia53I zF1DS(=|) zZDQ6HC)O21MUBUjEdx$cUTPAj{i0Jul+z*|9U;VbQ;01$qFHZ7nfcWeyQ#^?w?ql5 zBQOfP_C@VU1~^{4oE(%rJac_idf{(Z;~P>UDXFN`Zo|^Q;{|bKlvR6+#l!?_f=;)B z_yle@)jI^Jt(m>DGFfzbp)uNs*!}>*t#qCKAf@^~!^>e`R}-+k1fF~|CCb;sr*tb* zYN=f41T+p1Jw`ztB33mA4YK#BS8S=3ScHaTJ`gp17`)KF?CUBwo1BIVI${gzz&wtL zD+ZLWaI9c%2XB1m@Nm7)8p=6XGW_nI@bF~LH}%ppuDwG?hla=#6Oa0uk98H~Xb;Im zlcqdCcBqx@WeYi}q82f-ysW+VH;OG2Ihs{15p?xT*0GU`UhUV$|9)T$2BXGEbDrJi zH7|+(Ht|rE2JG_zZeYjT^XV_rz?Sz8j{=(!Zmo;)U$GLIJ1dRt*c1jMGSNFp>^K8J z@{z48`5K~Kn~8j%T_e#&=Z{+DVhqZ`M;^Dg>ixZ$lJv@%0b^qBdQlpA71y`T(+H+$ z@E0S(@o+$QQupq=s0R_VvfGm=W>rd6Ro(hxHAm$q6kz$p1JWw-WRC_@+z%AqhauXk z(?g{36h;RcX$W9YHuwLYki0yG?t?=EZ+rOI;6V3$`dPm6Xn;z76q3Sn@CV3pPQ;&n zfqqaxeZBMYpZp4^e5BQvKi!nD001|TR*mssCBK=Zq~4b6mm&Rbseov#AgCIZDPX8{ zl!e-VUWg?W-^e&?zOMrY0SWn_SWdp}0R=V)B4U=nvNr0nrol$vEqtG&&|Uig#`_7* zhFtdch1%1;wYsw4??20-A=qA8bgk<(?l%}DedwRmZU%o3KcVI;p%M`ZMOhHt!@FD0 zUA#AH)OTP&J?9>HCP=`!FCFq!VX^6;n28@m3*&uKQWK?tJp^K*-YWUsYuV1drG08v z=Q>4C4Afa^|K?=6y*S^rA8)#)C@b#-QBtkio7?m`Z~zSRf4{^3Lok-ZdhDpJ@k)k0 zXng`}G0BYHVow)0*UA3RdxpmA*lR4;7ew7hwP)mt#Rhu;J{0Pin7pDxdHc7iqrl3~ zjBkdL@V3O~rK@p*8rviTmi+{w~qWWdp@k2Kq({p2k`PXo_Rpw;xgGgE92kt0_EO z!MjRGEI*5eAWvZi5@`9)2(+t zgB(5k?{V3Io>Q%4TWjbI1Qi2$H~>q40DEwhDU6%QY+2V^we_1AHcRsKsotAv zFKUzn8MoFk&lfPKe+iyH5hHMPEbCs(bJ3|Eb#3-^KA%c}pf=M>PmhL$U*T11F6cF7 zB`4rI2$IGT7Z$YK-AU&0{kBmY?##P{LNuJYD#vhf zssVSlw+D#(d|FinWi8ro@?>n!Huf4kR?rkP3#lj+ezkg+uEuI<*EsAuul+?iPgp8n zUS0MDWT_ExXn3(fVy()=q6LC&@nF1D#P#~qfA4uFOcRD*DO7DL=-7pa%ffQ-OCE1$ zoMEPAwsA2pa#~*P3#uM@q*icI3Qf6cZ`Hiob?Y9@LOo^#NXWi*8Axa;D2UgCnapWl z`)umh=7RfK37>M;S9V|oj{Oewi+z4n@R!}uDc}La6BCk=nv5hm^pcJG{{744$y1`e z+19kNwlz{sZa(-YKN_Sin*SCzJVto+FVJix6UoE7+D~UVN(19$bZpmaE@>pJdS!?2 zy3ZMEpDHJXU z0x%A7uv~+?w)X1#m*)VsZHfMVL}scK^hp&E;)o<70&JH(oK|me@j~nDAT&<2YD@?bs5#V{7hFb5%f-=q5t}_%-X!C=dDE{a)$+6X8|8n z*`ISjv_kiywMMf;Iom^|qo+J81@>pxg?{C!GFdDy`JR%JqXO~~>r4EwhrxiOo^xs% zJPZ}~hKjf*Gr>nY>zA9bsty)QL(}Ai{(JHz7tz&u{as&^XSM6l>TlxnsZZ*|Sr7U@ z3)bt@)nM0sk^e8fpBx9xXl_pr)t;=ab^|V7VHk|GVjFSW3rJ?mn%^AiR^o3g&+gjd zdguO2N=iN&V3i#rP^8${kFD)qT(7s&qqt%W3C>51JKK=^}o8HHkGfaP3?2? z@Y(a`gL2hgYQ{8Y;ukCpG5FR8k&De)$hn%p40|_-lC(H#8Ki^5HR`0g4db<#S%3ya zatO_(Yl!37L0@XNEi~%2Hce~2g70VjdHdhPfHYYt$tt-nF6ZI?+g=q)3DTXLt3A9q zAKec?yO>`tA!f=4q&-5GCFLvDBt{W9cGaBsF7 z_ubXDJU`P^c8QOQ(wy!}w)^E9*Y!mI2MKfC5Mh5O_p8~juC$fl8W6oJuPut6+Z*+q z;7oRG*Bwn>jjPMX^0GBll~E1L2RWG`zGALnZ0aK6>4M(0sq zKI^WIz!e0dttl!Im?OgBg6sM)cy~igYX?>@#0^?q*REvz#`L1JW}_}oN>!R)|J&E8 z6LMk&@r`5PD_Ew5c>-^K&w#J~Kq8uays>(^N`*SVpgV>Ns*5eiXM^Lu`04Ct5elyn zQ<#r-aJF}V$%0$*htMvSi3x}O+Wg|D>z%<|uD|$<7z(y6tji+)EHQBZSJ1x{=gO2? zh9-X5>u+f-&5wQK5+I)()n3>21rUF5bx03s;Nhut!wFOh?;Fl4REqg6y+3E_8+b4W zgvP$u@5VB_qP}uhJd3UKP5BP1nM9qP4*gG5%Co(9j!97w=p@&tDT0KLh5I6%#0soF zSuD=&6cZ7tw49RlBs_)2e0j`27sqFZU%&K=j};YJQg|JdGfmUf5I}x?tYU@YKrMVg1~J4l43-P0?Lw6vD{FuZcuRMf$FAp zLZrLnCgpc=DSo=6o)g4Y=Ac~g`;Qdr!39=clJ}VvW_H3}qyR%}KY5>1cbfrX;DK=o z=XC&@wRG=_fprtSg3J8w{=*M1g;DEgG+JkvMBT=Wc#b+a36H)5eM$IgJNJMicX*r~ zbuWzvNOIstOFEc_U9W^(|2fR;W67i~7b>exAYBEZxN z7*y>8f~RU>j0WC37U z5j6+|Y=F9k0p@tVg9{5LCMFwN^I+Uzd#Uw}?OY9Nwg)9rqT1=GNlFgH6AFs(xHv)} zPHqCR48G)zKn*?R<>i^=oPhb6g!J@5J&;rG<=Am^0WV8G1J|igcM4|b!6+M7z$FlG zIXrDmO!7RD`}NMgrNBm1;oXL3`}1%oB{1(bQavVW9mD-oIY;y-P+cy|MktL@7tY2| zV;W#q$P595+e};ng4f`>*(^+22Hagopq93`53b#f;L{3Ud=V=sPz4}8g0vY38_hNa zK-io5s7y>nB^QcEhC@Jr8g}ulqX$Y3mN7oixBi>DtF)M41R+2mphJYm2qx;liq?N~ zud4zvKu$@?6tUFm2_*z2{+!3K>mC}x6QQ0MDqe7*$RcoQIk7PD2kLWRP`J@X>~bS< zIGryVObf(VdZVM=N1*1j~NBCHDEIO8<_98U)K2%l|p3`z_9hxr%%_vz}fX%Qi15> z3@nKmpPrt+XvxK|q3_8AEJFisI*oCz78brhjbe%29>w{<$f>9Xriwo51$6;k7HDeU zf%ZmmL)F#Y-MC*jpUP#3F_gk-{yUz5LS%XJFF6=JAx#5S1T|9&B3jUC11HV1WN_-6 zIOb2KjCWwZ6{umkV1mmt`vK~jBakcmI|4D&UfWWG$tl#3U(VfiIM8TtQ7X#+c_hjc z`r4=m=WK7bTar+u2Q2mS7K!jY>ZS0!KI%U@Zu+OO)q1|90Iu|bAullWv(4#al#Rln zQc=S|$U8baV+L9!&^+O@1uNiw-Sd|(V}NT=qqaauvz-1lEe-MR3c 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..c741ba96 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 @@ -139,4 +175,4 @@ def update_trajectory_points( collected_data_counts_of_vel_acc, collected_data_counts_of_vel_steer, ): - return nearestIndex, *self.return_trajectory_points(yaw_offset, rectangle_center_position) + return nearestIndex, *self.return_trajectory_points(yaw_offset, rectangle_center_position) \ No newline at end of file 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..05c61346 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. ) @@ -818,9 +826,9 @@ def __init__(self, step: float, param_dict): self.trajectory_nearly_straight_clock_wise[self.steer_list[i]] = trajectory # Generate and store counterclockwise trajectories by reversing the clockwise trajectory. - self.trajectory_nearly_straight_counter_clock_wise[self.steer_list[i]] = ( - reverse_trajectory_segment(trajectory) - ) + self.trajectory_nearly_straight_counter_clock_wise[ + self.steer_list[i] + ] = reverse_trajectory_segment(trajectory) # Generate trajectories for changing directions (turning). @@ -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,77 @@ 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 +1255,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( @@ -1239,4 +1306,4 @@ def get_boundary_points(self): boundary_points.append([point_x, point_y]) # Store the boundary points as a numpy array - self.boundary_points = np.array(boundary_points) + self.boundary_points = np.array(boundary_points) \ No newline at end of file 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_pure_pursuit_trajectory_follower_acceleration_cmd.py b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py index c8ce777c..e0125369 100755 --- a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py +++ b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py @@ -505,8 +505,6 @@ def control(self): acceleration_cmd = self.acceleration_cmd cmd[0] = acceleration_cmd - self.get_logger().info("cmd_mode : " + self.CONTROL_MODE) - cmd_without_noise = 1 * cmd tmp_acc_noise = self.acc_noise_list.pop(0) 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 8115dd1f..bebb1a55 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -445,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( [ @@ -553,7 +537,7 @@ def callback_trajectory_generator(self): index_array_near = np.argsort(distance) if self.present_operation_mode_ == 3: self.nearestIndex = index_range[index_array_near[0]] - # set target velocity + # 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 @@ -572,13 +556,19 @@ def callback_trajectory_generator(self): self.mask_vel_steer, ) elif self.CONTROL_MODE == "actuation_cmd": - 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, - ) + 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" From f88f2ce10ede5583d5bb1d7875417ecf26a91c1f Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Thu, 2 Jan 2025 03:53:37 +0900 Subject: [PATCH 15/17] Update README Signed-off-by: Yoshihiro Kogure --- control_data_collecting_tool/README.md | 8 +++++++- control_data_collecting_tool/config/common_param.yaml | 6 +++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 01122513..4e605095 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -475,7 +475,9 @@ ROS 2 parameters which are common in all trajectories (`/config/common_param.yam | `NUM_BINS_STEER` | `int` | Number of bins of steer in heatmap | 20 | | `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_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 | @@ -487,6 +489,10 @@ 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 | diff --git a/control_data_collecting_tool/config/common_param.yaml b/control_data_collecting_tool/config/common_param.yaml index 82c65e1f..2e076e3d 100644 --- a/control_data_collecting_tool/config/common_param.yaml +++ b/control_data_collecting_tool/config/common_param.yaml @@ -19,8 +19,8 @@ NUM_BINS_A: 10 NUM_BINS_ABS_STEER_RATE: 5 NUM_BINS_JERK: 10 - NUM_BINS_ACCEL_PEDAL_INPUT: 10 - NUM_BINS_BRAKE_PEDAL_INPUT: 12 + NUM_BINS_ACCEL_PEDAL_INPUT: 8 + NUM_BINS_BRAKE_PEDAL_INPUT: 16 V_MIN: 0.0 V_MAX: 11.5 STEER_MIN: -0.6 @@ -32,7 +32,7 @@ JERK_MIN: -0.5 JERK_MAX: 0.5 ACCEL_PEDAL_INPUT_MAX: 0.4 - ACCEL_PEDAL_INPUT_MIN: 0.01 + ACCEL_PEDAL_INPUT_MIN: 0.0 BRAKE_PEDAL_INPUT_MAX: 0.8 BRAKE_PEDAL_INPUT_MIN: 0.0 From 1facaf9486b3fc0125067e08819f7a780d8d7b7e Mon Sep 17 00:00:00 2001 From: Yoshihiro Kogure Date: Thu, 2 Jan 2025 04:03:26 +0900 Subject: [PATCH 16/17] pre-commit run Signed-off-by: Yoshihiro Kogure --- .../scripts/courses/base_course.py | 4 ++-- .../scripts/courses/reversal_loop_circle.py | 20 ++++++++++++------- .../data_collecting_trajectory_publisher.py | 4 +++- 3 files changed, 18 insertions(+), 10 deletions(-) diff --git a/control_data_collecting_tool/scripts/courses/base_course.py b/control_data_collecting_tool/scripts/courses/base_course.py index c741ba96..8246c76b 100644 --- a/control_data_collecting_tool/scripts/courses/base_course.py +++ b/control_data_collecting_tool/scripts/courses/base_course.py @@ -108,7 +108,7 @@ def get_target_velocity( mask_vel_steer, ): return None - + def get_target_pedal_input( self, nearestIndex, @@ -175,4 +175,4 @@ def update_trajectory_points( collected_data_counts_of_vel_acc, collected_data_counts_of_vel_steer, ): - return nearestIndex, *self.return_trajectory_points(yaw_offset, rectangle_center_position) \ No newline at end of file + return nearestIndex, *self.return_trajectory_points(yaw_offset, rectangle_center_position) 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 05c61346..29594441 100644 --- a/control_data_collecting_tool/scripts/courses/reversal_loop_circle.py +++ b/control_data_collecting_tool/scripts/courses/reversal_loop_circle.py @@ -826,9 +826,9 @@ def __init__(self, step: float, param_dict): self.trajectory_nearly_straight_clock_wise[self.steer_list[i]] = trajectory # Generate and store counterclockwise trajectories by reversing the clockwise trajectory. - self.trajectory_nearly_straight_counter_clock_wise[ - self.steer_list[i] - ] = reverse_trajectory_segment(trajectory) + self.trajectory_nearly_straight_counter_clock_wise[self.steer_list[i]] = ( + reverse_trajectory_segment(trajectory) + ) # Generate trajectories for changing directions (turning). @@ -1198,16 +1198,22 @@ def get_target_pedal_input( 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 + 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 + 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 + 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 @@ -1306,4 +1312,4 @@ def get_boundary_points(self): boundary_points.append([point_x, point_y]) # Store the boundary points as a numpy array - self.boundary_points = np.array(boundary_points) \ No newline at end of file + self.boundary_points = np.array(boundary_points) 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 bebb1a55..9e210083 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -568,7 +568,9 @@ def callback_trajectory_generator(self): 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}") + 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" From 4da8eb5a68aee8767ae6623ad27a5e98defbb63b Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 9 Jan 2025 15:20:20 +0900 Subject: [PATCH 17/17] Update control_data_collecting_tool/README.md --- control_data_collecting_tool/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 4e605095..e4f0b0e9 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -148,7 +148,7 @@ This package provides tools for automatically collecting data using pure pursuit a. Start the Tool by using the following command ```bash - ros2 run control_data_collecting_tool data_collecting_acceleration_cmd + 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: