diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index d88601497096d..dad351dec40e8 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -233,6 +233,7 @@ + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index b31506918a2db..49749cd1299bd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -4,8 +4,3 @@ backward_path_length: 5.0 behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 - max_accel: -2.8 - max_jerk: -5.0 - system_delay: 0.5 - delay_response_time: 0.5 - is_publish_debug_path: false # publish all debug path with lane id in each module diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 3c4f4c3fdd64c..399762f0b8607 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -11,6 +11,7 @@ + @@ -26,7 +27,6 @@ - @@ -53,6 +53,7 @@ + @@ -68,7 +69,5 @@ - - diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json index 67c9c06104b40..80044d5c6af03 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json @@ -21,47 +21,17 @@ "default": "1.0", "description": "the output path will be interpolated by this interval" }, - "max_accel": { - "type": "number", - "default": "-2.8", - "description": "(to be a global parameter) max acceleration of the vehicle" - }, - "system_delay": { - "type": "number", - "default": "0.5", - "description": "(to be a global parameter) delay time until output control command" - }, - "delay_response_time": { - "type": "number", - "default": "0.5", - "description": "(to be a global parameter) delay time of the vehicle's response to control commands" - }, "stop_line_extend_length": { "type": "number", "default": "5.0", "description": "extend length of stop line" - }, - "max_jerk": { - "type": "number", - "default": "-5.0", - "description": "max jerk of the vehicle" - }, - "is_publish_debug_path": { - "type": "boolean", - "default": "false", - "description": "is publish debug path?" } }, "required": [ "forward_path_length", "behavior_output_path_interval", "backward_path_length", - "max_accel", - "system_delay", - "delay_response_time", - "stop_line_extend_length", - "max_jerk", - "is_publish_debug_path" + "stop_line_extend_length" ], "additionalProperties": false } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index cc2f4bf3b96b4..1dfa3163b5b6f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -49,6 +49,8 @@ std::shared_ptr generateNode() const auto autoware_test_utils_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto behavior_velocity_planner_common_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common"); const auto behavior_velocity_planner_dir = ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); const auto velocity_smoother_dir = @@ -82,25 +84,27 @@ std::shared_ptr generateNode() node_options.parameter_overrides(params); autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", - velocity_smoother_dir + "/config/Analytical.param.yaml", - behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config("blind_spot"), - get_behavior_velocity_module_config("crosswalk"), - get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config("detection_area"), - get_behavior_velocity_module_config("intersection"), - get_behavior_velocity_module_config("no_stopping_area"), - get_behavior_velocity_module_config("occlusion_spot"), - get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config("speed_bump"), - get_behavior_velocity_module_config("stop_line"), - get_behavior_velocity_module_config("traffic_light"), - get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("no_drivable_lane")}); + node_options, + {autoware_test_utils_dir + "/config/test_common.param.yaml", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", + get_behavior_velocity_module_config("blind_spot"), + get_behavior_velocity_module_config("crosswalk"), + get_behavior_velocity_module_config("walkway"), + get_behavior_velocity_module_config("detection_area"), + get_behavior_velocity_module_config("intersection"), + get_behavior_velocity_module_config("no_stopping_area"), + get_behavior_velocity_module_config("occlusion_spot"), + get_behavior_velocity_module_config("run_out"), + get_behavior_velocity_module_config("speed_bump"), + get_behavior_velocity_module_config("stop_line"), + get_behavior_velocity_module_config("traffic_light"), + get_behavior_velocity_module_config("virtual_traffic_light"), + get_behavior_velocity_module_config("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt index 9cb992312f52a..f76fcfa31c240 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -26,4 +26,6 @@ if(BUILD_TESTING) ) endif() -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE + config +) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml new file mode 100644 index 0000000000000..aff2aec9cfa29 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + max_accel: -2.8 + max_jerk: -5.0 + system_delay: 0.5 + delay_response_time: 0.5 + is_publish_debug_path: false # publish all debug path with lane id in each module diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json new file mode 100644 index 0000000000000..2468b71aa9be1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json @@ -0,0 +1,59 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Behavior Velocity Planner Common", + "type": "object", + "definitions": { + "behavior_velocity_planner_common": { + "type": "object", + "properties": { + "max_accel": { + "type": "number", + "default": "-2.8", + "description": "(to be a global parameter) max acceleration of the vehicle" + }, + "system_delay": { + "type": "number", + "default": "0.5", + "description": "(to be a global parameter) delay time until output control command" + }, + "delay_response_time": { + "type": "number", + "default": "0.5", + "description": "(to be a global parameter) delay time of the vehicle's response to control commands" + }, + "max_jerk": { + "type": "number", + "default": "-5.0", + "description": "max jerk of the vehicle" + }, + "is_publish_debug_path": { + "type": "boolean", + "default": "false", + "description": "is publish debug path?" + } + }, + "required": [ + "max_accel", + "system_delay", + "delay_response_time", + "max_jerk", + "is_publish_debug_path" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/behavior_velocity_planner_common" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/scripts/dilemma_zone_plotter.py b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/scripts/dilemma_zone_plotter.py index 55bfbeff893b1..072979a2cef48 100755 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/scripts/dilemma_zone_plotter.py +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/scripts/dilemma_zone_plotter.py @@ -38,7 +38,7 @@ def get_params_from_yaml(): # get parameters from behavior velocity planner behavior_vel_yaml_file_path = os.path.join( autoware_launch_package_path, - "config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml", + "config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner_common.param.yaml", ) with open(behavior_vel_yaml_file_path, "r") as yaml_file: params = yaml.safe_load(yaml_file)